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Preface 



The purpose of this volume in Advances in Robot Manipulators is to encourage and inspire 
the continual invention of robot manipulators for science and the good of humanity. The 
concepts of artificial intelligence combined with the engineering and technology of feedback 
control, have great potential for new, useful and exciting machines. The concept of eclecticism 
for the design, development, simulation and implementation of a real time controller for an 
intelligent, vision guided robots is now being explored. The dream of an eclectic perceptual, 
creative controller that can select its own tasks and perform autonomous operations with 
reliability and dependability is starting to evolve. We have not yet reached this stage but a 
careful study of the contents will start one on the exciting journey that could lead to many 
inventions and successful solutions. 



Editor: 
Ernest Hall 
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Abstract 

Minimally Invasive Surgery represents the future of many types of medical interventions such 
as keyhole neurosurgey or transluminal endoscopic surgery. These procedures involve inser- 
tion of surgical instruments such as needles and endoscopes into human body through small 
incision/ body cavity for biopsy and drug delivery. However, nearly all surgical instruments 
for these procedures are inserted manually and there is a long learning curve for surgeons to 
use them properly. Many research efforts have been made to design active instruments (endo- 
scope, needles) to improve this procedure during last decades. New robot mechanisms have 
been designed and used to improve the dexterity of current endoscope. Usually these robots 
are flexible and can pass the constrained space for fine manipulations. In recent years, a con- 
tinuum robotic mechanism has been investigated and designed for medical surgery. Those 
robots are characterized by the fact that their mechanical components do not have rigid links 
and discrete joints in contrast with traditional robot manipulators. The design of these robots 
is inspired by movements of animals' parts such as tongues, elephant trunks and tentacles. 
The unusual compliance and redundant degrees of freedom of these robots provide strong 
potential to achieve delicate tasks successfully even in cluttered and unstructured environ- 
ments. This chapter will present a complete application of a continuum robot for Minimally 
Invasive Surgery of colonoscopy. This system is composed of a micro-robotic tip, a set of po- 
sition sensors and a real-time control system for guiding the exploration of colon. Details will 
be described on the modeling of the used pneumatic actuators, the design of the mechanical 
component, the kinematic model analysis and the control strategy for automatically guiding 
the progression of the device inside the human colon. Experimental results will be presented 
to check the performances of the whole system within a transparent tube. 



* Corresponding author. 
gang.chen@unilever.com 



Advances in Robot Manipulators 



1. Introduction 

Robotics has increasingly become accepted in the past 20 years as a viable solution to many 
applications in surgery, particularly in the field of Minimally Invasive Surgery (MIS)Taylor & 
Stoianovici (2003). Minimally Invasive Surgery represents the future of many types of medical 
interventions such as keyhole neurosurgery or transluminal endoscopic surgery. These pro- 
cedures involve insertion of surgical instruments such as needles and endoscopes into human 
body through small incision/ body cavity for biopsy and drug delivery. However, nearly all 
surgical instruments for these procedures are inserted manually and they are lack of dexterity 
in small constrained spaces. As a consequence, there is a long learning curve for surgeons to 
use them properly and thus risks for patients. Many research efforts have been made to im- 
prove the functionalities of current instruments by designing active instruments (endoscope, 
needles) using robotic mechanisms during the last decades, such as snake robot for throat 
surgery Simaan et al. (2004) or active cannula Webster et al. (2009). Studies are currently un- 
derway to evaluate the value of these new devices. Usually these robots are micro size and 
very flexible so that they can pass the constrained space for fine manipulations. Furthermore, 
how to steer these robots into targets safely during the insertion usually needs additional sen- 
sors, such as MRI imaging and US imaging, and path planning algorithms are also needed to 
be developed for the intervention. 

Colonoscopy is a typical MIS procedure that needs the insertion of long endoscope inside 
the human colon for diagnostics and therapy of the lower gastrointestinal tract including the 
colon. The difficulty of the insertion of colonoscope into the human colon and the pain of the 
intervention brought to the patient hinders the diagnostics of colon cancer massively. This 
chapter will present a novel steerable robot and guidance control strategy for colonoscopy 
interventions which reduces the challenge associated with reaching the target. 

1.1 Colonoscopy 

Today, colon cancer is an increasing medical concern in the world, where the second frequent 
malignant tumor is found in industrialized countries Dario et al. (1999). There are several 
different solutions to detect this kind of cancer, but only colonoscopy can not only make diag- 
nostics, but make therapy. Colonoscopy is a procedure which is characterized by insertion of 
endoscopes into the human colon for inspection of the lower gastrointestinal tract including 
the colon in order to stop or to slow the progression of the illness. The anatomy of the colon 
is showed in Fig. 1. 

The instrument used for diagnostics and operation of the human colon is called endoscope 
(also colonoscope) which is about 1.5cm in diameter and from 1.6 to 2 meters in length. 
Colonoscopy is one of the most technically demanding endoscopic examinations and tends 
to be very unpopular with patients because of many sharp bends and constrained workspace. 
The main reason lies in the characteristics of current colonoscopes, which are quite rigid and 
require the doctor to perform difficult manoeuvres for long insertion with minimal damage of 
the colon wall Fukuda et al. (1994); Sturges (1993). 

1.2 State of the art: Robotic colonoscopy 

Since the human colon is a tortuous "tube" with several sharp bends, the insertion of the 
colonoscope requires the doctor to exert forces and rotations at shaft outside of the patient, 
thus causing discomfort to the patient. The complexity of the procedure for doctors and 
the discomfort experienced by the patient of current colonoscopies lead many researchers 
to choose the automated colonoscopy method. In Phee et al. (1998), the authors proposed the 
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Fig. 1. The anatomy of the colon 



concept of automated colonoscopy (also called robotic colonoscopy) from two aspects: loco- 
motion and steering of the distal end, which are the two main actions during a colonoscopy. In 
order to facilitate the operation of colonoscopy, some studies on the robotic colonoscopy have 
been carried out from these two aspects. Most current research on autonomous colonoscopies 
have been focused on the self-propelled robots which utilize various locomotion mecha- 
nisms Dario et al. (1997); Ikuta et al. (1988); Kassim et al. (2003); Kumar et al. (2000); Menci- 
assi et al. (2002); Slatkin & Burdick (1995). Among them, inchworm-like locomotion attracted 
much more attention Dario et al. (1997); Kumar et al. (2000); Menciassi et al. (2002); Slatkin 
& Burdick (1995). However, most of the current inchworm-based robotic systems Dario et al. 
(1997); Kumar et al. (2000); Menciassi et al. (2002); Slatkin & Burdick (1995) showed low effi- 
ciency of locomotion for exploring the colon because of the structure of the colon wall: slip- 
pery and different diameters at each section.Another aspect work that could improve the per- 
formance of current colonoscopies is to design an autonomous steering robot for guidance 
inside the colon during the colonoscopy. Fukuda et al. (1994) proposed Shape Memory Alloy 
(SMA) based bending devices, called as Micro- Active Catheter (MAC), with two degrees of 
freedom. With three MACs connected together in series, an angle of bend of nearly 80° is 
possible. In Menciassi et al. (2002), a bendable tip has been also designed and fabricated by 
using a silicone bellows with a length of 30mm. It contains three small SMA springs with a 
120° layout. This device allows a 90° bending in three directions. These flexible steering tips 
are the only parts of the whole self-propelling robots, however those works did not focus on 
how to control this special robot to endow it with a capability for autonomous guidance Kim 
et al. (2006); Kumar et al. (2000); Menciassi et al. (2002); Piers et al. (2003). Since 2001, there is 
another method to perform colon diagnostics: capsule endoscopy (n.d.a;n). With a camera, a 
light source, a transmitter and power supply integrated into a capsule, the patient can swallow 
and repel it through natural peristalsis without any pain. Despite capsule endoscopy advan- 
tages, it does not allow to perform the diagnostics more thoroughly and actively. Recently, 
different active locomotion mechanisms have been investigated and designed to address this 
problem, such as clamping mechanism Menciassi et al. (2005), SMA-based Gorini et al. (2006); 
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Kim et al. (2005), magnet-based Wang & Meng (2008) locomotion and biomimetic geckoGlass 
etal. (2008). 

1.3 An approach to steering robot for colonoscopy 

The objective of our work in this chapter is original from all the works from other laborato- 
ries, which is to design a robot with high dexterity capable of guiding the progression with 
minimal hurt to the colon wall. Our approach emphasizes a robotic tip with a novel design 
mounted on the end of the traditional colonoscope or similar instruments. The whole system 
for semi-autonomous colonoscopy will be presented in this chapter. It is composed of a micro- 
tip, which is based on a continuum robot mechanism, a proximity multi-sensor system and 
high level real-time control system for guidance control of this robot. The schema of the whole 
system, called Colobot, is shown in Fig. 2. Section 2 briefly presents the Colobot and its prox- 
imity sensor system. Then section 3 will present model analysis of Colobot system and the 
validation of kinematic model in section 4. In section 5 guidance control strategy is presented 
and control architecture and implementation is then described. Finally, experimental results in 
a colon-like tube will be presented to verify the performance of this semi-autonomous system. 
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Fig. 2. The scheme of the whole system 



2. Micro-robotic tip: Colobot 

Biologically-inspired continuum robots Robinson & Davies (1999) have attracted much inter- 
est from robotics researchers during the last decades to improve the capability of manipu- 
lation in constrained space. These kinds of systems are characterized by the fact that their 
mechanical components do not have rigid links and discrete joints in contrast with traditional 
industry robots. The design of these robots are inspired by movements of animals' parts such 
as tongues, elephant trunks and tentacles etc. The unusual compliance and redundant degrees 
of freedom of these robots provide strong potential to achieve delicate tasks successfully even 
in cluttered and/or unstructured environments such as undersea operations Lane et al. (1999), 
urban search and rescue, wasted materials handling Immega & Antonelli (1995), Minimally 
Invasive Surgery Bailly & Amirat (2005); Dario et al. (1997); Piers et al. (2003); Simaan et al. 
(2004). The Colobot Chen et al. (2006) designed for our work, is a small-scaled continuum 
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robot. Due to the size requirement of the robot, there are challenges on how to miniaturize 
sensor system integrated into the small-scale robot to implement automatic guidance of pro- 
gression inside the human colon. This section will present the detailed design of the Colobot 
and its fibre-optic proximity sensor system. 

2.1 Colobot 

The difference between our robotic tip and other existing continuum robots is the size. Our 
design is inspired by pioneer work Suzumori et al. (1992) on a flexible micro-actuator (FMA) 
based on silicone rubber. Fig. 3(a) shows our design of the Colobot. The robotic tip has 3 




Active chamber 17mm 




Passive chamber 
(a) Colobot (b) Cross section of Colobot 

Fig. 3. Colobot and its cross section 



DOF (Degree of Freedom), which is a unique unit with 3 active pneumatic chambers regu- 
larly disposed at 120 degrees apart. These three chambers are used for actuation; three other 
chambers shown in Fig. 3(b) are designed to optimize the mechanical structure in order to 
reduce the radial expansion of active chambers under pressure. The outer diameter of the tip 
is 17 mm that is lesser than the average diameter of the colon. The diameter of the inner hole 
is 8mm, which is used in order to place the camera or other lighting tools. The weight of the 
prototype is 20 grams. The internal pressure of each chamber is independently controlled by 
using pneumatic jet-pipe servovalves. The promising result obtained from the preliminary 
experiment showed that this tip could bend up to 120° and the resonance frequency is 20 Hz. 

2.2 Modeling and experimental characterization of pneumatic servovalves 

During an electro-pneumatic control, the follow up of the power transfer from the source to 
the actuator is achieved through one or several openings with varying cross-section called 
restrictions: this monitoring organ is the servovalve Sesmat (1996). The Colobot device is 
provided by three jet pipe micro-servovalves Atchley 200PN Atchley Controls, jet Pipe cata- 
logue (n.d.), which allow the desired modulation of air inside the different active chambers 
in Fig. 3(b). In this component, a motor is connected to an oscillating nozzle, which deflects 
the gas stream to one of the two cylinder chambers (Fig. 4(a)). A voltage/current amplifier 
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allows to control the servovalves by the voltage Atchley (1982). A first pneumatic output of 
this component is directly connected to one of the robot chambers, and a second output is 
left unconnected. A sensor pressure (UCC model PDT010131) (Fig. 4(b)) is used to measure 
the pressure in each of the three Colobot robot chambers. The measured pressure, comprised 
between and 10 bars, was used to determine the servovalve control voltage. 




(a) Atchley servovalve 200PN 
Fig. 4. Atchley servovalve and pressure sensor 



(b) Pressure sensor 



As the three servo valves used for the COLOBOT actuator are identical, a random servovalve 
was chosen for the mass flow and pressure characterization. The pressure gain curve is the 
relationship between the pressure and the current control when the mass flow rate is null. 
It is performed by means of the pneumatic test bench shown in Fig. 5. A manometer was 
placed downstream of the servovalve close by the utilization orifice in order to measure the 
pressures. Fig. 6 shows the pressure measurements P n and Pp carried out for an increasing and 
a decreasing input current. It appears that the behavior of the servovalve is quite symmetric 
but with a hysteresis cycle. Arrival in stop frame couple creates pressure saturation at -18 
mA, respectively +18 mA, for the negative current, respectively for the positive current. In 
the Fig. 5, we substitute the manometer on the test bench for a static mass flow-meter to plot 
the mass flow rate gain curve (mass flow rate with respect to the input current). This curve 
presented in Fig. 7 shows a non linear hysteresis. 

Because of the specific size of Colobot's chambers, the experimental mass flow rate inside 
the chamber is very small, the current input and the pressure variations are small enough to 
neglect the hysteresis and consider linear characteristics for Fig. 6 and Fig. 7. 

2.3 Optical Fibre proximity sensors 

The purpose of this robotic system is to guide the insertion of the colonoscope through the 
colon. So it is necessary to integrate the sensors to detect the position of the tip inside the colon. 
Due to the specific operation environments and the small space constraint, two important 
criteria must be taken into account to choose the distance sensors: 

• the flexibility and size of the colonoscope, 

• the cleanliness of the colon wall. 

Tests have been performed using ultrasound and magnetic sensors as well as optical fibre. We 
decided to use optical fibre because of its flexibility, small size, high resolution, and the possi- 
bility of reflecting light off the porcine intestinal wall [16]. This optical fibre system consists of 
one emission fibre and a group of four reception fibres (Fig. 8(a)). The light is emitted from a 
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cold light source and conveyed by transmission fibres. After reflection on an unspecified body 
in front of the emission fibre, the reception fibres surrounding the emission fibre detect the re- 
flected light. The amount of reflected light detected is a function of the distance between the 
sensor and the body. Fig. 8(b) shows the output voltage determined by the distance between 
the sensor and the porcine intestinal wall. This curve shows that the sensor's resolution is suf- 
ficient for detecting the intestinal wall up to 8 mm. Fig. 9 shows the Colobot integrated three 
fibre optic proximity sensors. The first optical fibre is placed in front of the first pneumatic 
chamber and the other two in front of their individual pneumatic chambers. 



Advances in Robot Manipulators 



0.15 
0.1 

0.05 




Control current (mA) 



Fig. 7. Mass flow gain characterization 




Emission optical fibre 
• Reception optical fibres 







.600 

I 400 

a 

re 

=200 


A 








fV 








r\ 


V 








^. 






1 

( 








) 2 4 6 8 

Distance between the sensor and porcine intestinal 
wall (mm) 



(a) Cross section of the optical fibre proximity (b) Characteristic of the optical fibre sensors 
sensors 



Fig. 8. Proximity sensors and its characterization 



3. Kinematic modeling the tip and the proximity sensor system 

This section will deal with the kinematic modeling of the robotic tip and the model of the 
optical fibre sensors. 

3.1 Kinematic analysis of the robotic tip 

Fig. 10 shows the robot shape parameters and the corresponding frames. The deformation 
shape of ColoBot is characterized by three parameters as done in our previous prototype 
EDORA Chen et al. (2005). It is worth to note that Bailly & Amirat (2005); Jones & Walker 
(2006); Lane et al. (1999); Ohno & Hirose (2001); Simaan et al. (2004); Suzumori et al. (1992) 
used almost the same set of parameters for the modeling: 

• L is the length of the virtual center line of the robotic tip 

• a. is the bending angle in the bending plane 
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Fig. 9. Prototype integrated with optical fibre proximity sensors 




Fig. 10. Kinematic parameters of Colobot 



• (p is the orientation of the bending plane 

The frame R u (O-xyz) is fixed at the base of the actuator. The X-axis is the one that passed by 
the center of the bottom end and the center of the chamber 1. The XY-plane defines the plane 
of the bottom of the actuator, and the z-axis is orthogonal to this plane. The frame R s (u, v, w) 
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is attached to the top end of the manipulator. So the bending angle a is defined as the angle 
between the o-z axis and o-w axis. The orientation angle <p is defined as the angle between 
the o-x axis and o-t axis, where o-t axis is the project of o-w axis on the plane x-o-y. Given 
the assumption that the shape at the bending moment is an arc of a circle, the geometry-based 
kinematic model Chen et al. (2005) relating the robot shape parameters to the actuator inputs 
(chamber length) is expressed as follows: 



Lu 



atan2 

2^/AT 
3r 



V3(L 2 - I 3 ) 
'L 3 + L 2 -2L! 



(1) 



where A^ = I4 + L 2 + L 3 — L\L 2 — i-2^3 — L3L1 and r is the radius of the Cobobot and 
direct kinematic equations with respect to the input pressures are represented by: 



1 



L/m 



(p = atan2 



1=1 



V3(/ 2 (P 2 )-/ 3 (P 3 )) 

/3(P 3 )+/2(-P 2 )-2/l(Pl) 



(2) 



where: 



Ap = fl(Plf + MPiT + fs(Pif - fliPlWl) - f 2 (P2)f3(P 3 ) - MP 3 )fl(Pl) 

The function /,(P,) (i = 1,2,3) shows the relationship relating the stretch length of the cham- 
ber to the pressure variation of the silicone-based actuator as described as: 



AL i =f i (P i ) 



(3) 



Where AL,- (i = 1, 2, 3) is the stretch length of each chamber with corresponding pressure and 
/; (i = 1, 2, 3) is a nonlinear function of P,. The corresponding results can be written as: 

tf ^ Xmin < ^ 1 < ^ Imax 

ALi = 37(Pj - P lmm ) 3 - 54(Pj - P lm ,„) 2 
-9.5(Pi - P lmm ) 

if Plmin < Pi < Plmax 

AL 2 = -9(P 2 - P 2mm ) 3 - 18(P 2 - P 2mm ) 2 
-ll(P 2 -P 2mm ) 

if P 3 min < P 3 < P?,max 
AL 3 = 0.8(P 3 - P 3min ) 3 
-34(P 3 - P 3min ) 



(4) 



8.9 (P 3 



L 3min ) 



where P/„, !H (i = 1, 2, 3) is the threshold of the working point of each chamber and their values 
equal: P\ m \ n = 0.7 bar, Pimin = 0.8 bar, Pzmin = 0-8 bar and Pj max (i = 1,2,3) is the maximum 
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pressure that can be applied into each chamber. The detailed deduction of these equations can 
be found in Chen et al. (2005). The Cartesian coordinates (x,y,z) of the distal end of Colobot 
in the task space related to the robot bending parameters is obtained through a cylindrical 
coordinate transformation: 

x = — (1 — cos a) cos <p 

y = — (1 — cos a) sin<p (5) 

L 

z = — sin a 
a 

And the state-space form of this model is given by: 

X = f(Q P ) (6) 

where X= (a, <p,L) T ,Q P = (Pi,P 2 ^ 3 ) T - 

3.2 Modeling and calibration of optical fibre sensors 

For the preliminary test of our system, a transparent tube will be used which will be detailed 
in section 6. So the distance model of the optical fibre sensors with respect to this tube needs 
to obtained before performing the test. Experimental methods are used to obtain the model 
of each sensor. The voltage (w,- in volts) with respect to the distance (dj in mm) between the 
sensor and the tube wall is measured. Fig. 11 shows the measurements and the approximation 
model of the third sensor. The model of each sensor is obtained as follows: 

-40 
Ml = 3^3 (7) 

-50 

112 = Ttd^Ti (8) 

" 3 = 17^23 (9) 

4. Validation of the kinematic model 

Since the kinematics of Colobot has been described as the relationship between the deflected 
shape and the lengths of the three chambers (three pressures of each chamber), the validation 
of the kinematic model needs to have a sensor to measure the deflected shape, i.e. the bending 
angle, the arc length and the orientation angle. This section first presents sensor choice and 
its experimental setup for determining these system parameters, and presents the validation 
of the static kinematic model. 

4.1 The sensor choice and experimental setup 

For most continuum style robots, the determination of the manipulator shape is a big prob- 
lem because of the dimension and the inability to mount measurement device for the joint 
angles. Although there are several technologies that could solve this problem for large size 
robots Ohno & Hirose (2001), they are difficult to implement on a micro-robot. Since a Carte- 
sian frame has been analyzed with relation to the deflected shape parameters, an indirect 
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Characteristics of optical fiber sensor 3 




Distance (mm) 



Fig. 11. modeling of optical fiber sensor 



method is used to validate the kinematic model with the 3D position measurement. For this 
purpose, an electromagnetic miniBIRD sensor is used for the experimental validation. 
MiniBIRD is a six degree-of-freedom (position and orientation) measuring device from As- 
cension Technology Corporation (n.d.c). It consists of one or more Ascension Bird electronic 
units, a transmitter and one or more sensors (Fig. 12). It offers full functionality of other mag- 
netic trackers, with miniaturized sensors as small as 5mm wide. For data acquisition, the 




Fig. 12. MiniBIRD 6 DOF magnetic sensor 



bottom of Colobot is bounded to a fixture and the sensor is placed on the top of Colobot, 
shown in Fig. 12. The transmitter is placed at a stationary position. Thus the position and 
orientation of top-end of Colobot are directly measured from the sensor receiver with relation 
to the transmitter, and then the position of top-end of the manipulator with relation to the 
bottom of the manipulator is calculated indirectly through reference transformation. 
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Fig. 13. Measurement configuration 



4.2 Validation of the static model 

Using the sensor configuration, an open-loop experiment was carried out to validate the static 
model of the bending angle and the orientation angle (Eq. 2). As for the validation of bend- 
ing angle, one orientation of Colobot movement is used for validation. The bending angle is 
directly measured from the miniBIRD sensor and compared with theoretical results from ac- 
tual pressure obtained from the proportional valves. As shown in Fig. 14, the bending angle 
concerning the chamber length and the chamber pressure respectively has almost the same 
characteristics compared with the actual measurements. 
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Fig. 14. Comparisons of the bending angle with relation to the chamber length and chamber 
pressure 



To check the orientation angle, the position in the XY frame coordinate of the top-end of 
Colobot are measured for the six principal manipulator directions. Firstly, expected pres- 
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sure combinations were used for Colobot to follow the six principal orientation angles 
(0°, 60°, 120°, 180°, 240°, 300°) while the bending angle varied from 0° to the maximum. Then 
the measured positions of the top-end of Colobot were plotted relative to the original posi- 
tion of Colobot without deformation. This experimental protocol leads to Fig. 15. This figure 
highlights that the six orientation angles are in accordance with the theoretical values except 
for high pressures in the chambers. 

Validation of orientation angle phi 



Y(mri) 

o' 




" 6G 50 -40 -30 -20 -10 10 20 30 40 50 

X(mm) 
Fig. 15. Comparison of the orientation angle: measurement and simulation 



4.3 Verification of the coupling between each chamber 

Section 4.2 validated the bending angle and orientation angle separately in static. However, 
most of the time the motion of the device results from the pressure differentials between each 
chamber, this is to say, the interaction of each chamber. So it is necessary to check this mutual 
interaction between each chamber. To achieve this goal, sinus reference signals of pressure 
with 120° delay are applied to each servovalve. They are employed to make Colobot turn 
around its vertical axis with a constant velocity (see the experimental setup Fig. 13) to see the 
mutual interaction of each chamber. By using miniBIRD, the endpoint coordinates of Colobot 
can be obtained in XOY plane. Thus the comparison between these coordinates and those 
obtained from the simulation of the kinematic model (Eq. 5) allows us to check if there are 
interactions between chambers on the elongation of the prototype. 

Two comparisons are then proposed in Figures 16 and 17. For the first case, three sinus signals 
of pressure with amplitude of 0.4 bar and an offset of 0.9 bar are applied in the chambers of 
the prototype. The path of the Colobot's endpoint is a form of triangle (Fig. 16) because these 
actuators of Colobot work across the threshold of their dead zones. For the latter case, three 
sinus signals of pressure with amplitude of 0.4 bar and an offset of 1.2 bar are applied in the 
chamber of Colobot. In this case, Colobot works in the working zone and the endpoint path 
of Colobot lead to a circular shape (Fig. 17). The lines in the outer layer are the simulation 
result from the kinematic model relating XY coordinates to the corresponding pressure of 
each chamber (Eq. 4). Since the characteristics of deformation under pressure is performed 
each chamber by each chamber independently (Eq. 4), the difference between the results 
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Fig. 16. Simulation et experimental results of the movement of the Colobot's tip (across dead 
zone) 



of simulation and the experimental results showed in Figure 16 and Figure 17 suggests that 
interactions exist among each chamber. These interactions are taken into account in section 4.4. 



4.4 Estimation of a correction parameter 

In this section, new parameters are chosen to represent the interactions between each cham- 
ber. Thus, six stiffness parameters are introduced to describe the coupling effect of stretching 
of one chamber to that of other two chambers. Let denotes ku the mutual stiffness that deter- 
mines the effect of P, (i=l,2,3) on the length of the chamber j (j = 1,2,3) (where i does not equal 
j). The coefficients are obtained by minimizing the difference between the operational coor- 
dinates (X s , Y s ) measured by miniBIRD and the operational coordinates (X m , Y m ) obtained by 
simulation of the kinematic model (Fig. 18). 

A classical non-linear optimization based on the Levenberg-Marquardt algorithm is pro- 
ceeded to adjust the unknown parameters A:,,. The cost criterion chosen is: 



J(kij) 



^(X s )2 + (Y s )2 - ^fc, ; (X m )2 + fy(Y m )2) 



(10) 



The numerical results roughly lead to the same coefficient k = 0.3 for the unknown parameters 
kj:. Thus the new expression of the kinematic model is given by: 



ALi =/i(Pi) + 0.3(/ 2 (P 2 )+/ 3 (P 3 )) 
AL 2 = / 2 (P 2 ) +0.3(/ 1 (P 1 ) +/ 3 (P 3 )) 
AL 3 =/ 3 (P 3 )+0.3(/ 1 (P 1 )+/ 2 (P 2 )) 



(11) 
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Comparison between the model without correction and experiment 
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Fig. 17. Simulation and experimental results of the movement of the endpoint of Colobot 




Fig. 18. Optimization model 



It is noteworthy that the elongation expression ALj (respectively AL2, AL3 ) in (Eq. 11) is 
equivalent to (Eq. 3) when the relative pressures P2 and P3 (respectively Pj, P3 and P\, Pj) are 
equal to zero. 

To check this new kinematic model a cross validation is made with three other experiments. 
Three sinus input pressures with amplitude from 0.1 bar to 0.3 bar are applied into three 
chambers of Colobot. The improved kinematic model with the correction coefficient k is used 
to a straightforward comparison with the sets of data. Results shown in Fig. 19 and Fig. 20 
are testimony to the behavior of the proposed model in these two cases. 
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Results of different pressure ampitude (one part in the deadzone) 
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Fig. 19. Verification of corrected model with different pressure inputs (across dead zone) 
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5. Guidance control strategy based on proximity multi-sensor system 
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Fig. 21. Position of Colobot inside the colon 



5.1 Guidance control strategy 

The objective of sensor-based guidance strategy is to calculate the safe position of the distal- 
end of Colobot compared to the colon wall in real-time based on the measurements of three 
distance sensors for guidance inside the colon. For the sake of simplicity but without loss of 
generality it is assumed that a colon is a cylindrical tube and its cross section is an ellipse at the 
sensor plane. Fig. 21 illustrates the sensor plane, the distal end of ColoBot and the colon axis. 
With these assumptions, the safe position will be the central axis of the colon. To approximate 
the colon axis, a method based on a circumscribed circle is proposed. Since three points D\, 
D2 and D3 (Fig. 22) of sensor measurements form a triangle, the center of the circumscribed 
circle of this triangle is chosen as the safe position. This approach iterates as following: 

• Three sensor measurements are collected. 

• Position P n in the frame R s (Fig. 22) is evaluated with these three measurements. 

• If P n is a safe position, then it's necessary to go back to the first step for the next period; 
otherwise, next the safe position P n +\ described in the Frame R u (Fig. 22) is calculated 
through the circumscribed method and is provided to the kinematic control for execu- 
tion. 

For more details about the guidance control strategy, please find the reference Chen et al. 
(2008). 



5.2 Guidance control architecture 

The control of Colobot is organized in three hierarchical levels, as shown in Fig. 23. The first 
level consists of local pressure control of each Colobot's chamber through three servovalves. 
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Pn: Initial position of colonoscope 
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Fig. 22. Computation of the safe position 



Three independent PI controllers are used to implement the closed-loop pressure control of 
the chamber. The position and orientation of Colobot are controlled at level 2 using an instan- 
taneous inverse Jacobian method. This section will describe the implementation detail. Level 
3 is the sensor-based planning for automatic navigation described in section 5.1. 

5.3 Formulation of task space control of Colobot 

After determining the desired trajectory from sensor-based planning, the kinematic control 
of Colobot will be described in this section. It should be noted that two variables are used 
to represent the position of Colobot inside the colon. However, the Colobot has 3 degrees of 
freedom. So this manipulator becomes redundant for the chosen task. The velocity kinematic 
equations are rewritten as following: 



X 
X 



■f(Qp) 
ax 



d(a,<p,L)dQ L 



d(cc,cp,L) 
X = hhl v Q v = 



JQ P 



dQi 



Qv 



where X = (x,y) 1 ,Q L = (Li,L 2 ,L 3 ) J ,Q P = (P 1 ,P 2 ,P 3 ) i and/ 
with relation to the three levels of pressure in the chambers. 



(12) 



JsJiJp is the Jacobian matrix 



5.4 Resolution of the inverse kinematics with redundancy 

In the case of a redundant manipulator with respect to a given task, the inverse kinematic 
problem admits infinite solutions. This suggests that redundancy can be conveniently ex- 
ploited to meet additional constraints on the kinematic control problem in order to obtain 
greater manipulability in terms of the manipulator configurations and interaction with the 
environment. A viable solution method is to formulate the problem as a constrained linear 
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Computation of position P n+ i and inverse kinematics control 



Next position P n 



Fig. 23. sensor-based planning and guidance control procedure 



optimization problem. Work on resolved-rate control Whitney (1969) proposed to use the 
Moore-Penrose pseudo inversion of the Jacobian matrix as: 



Qp = I + x = (f(j} T )- 1 )x 



(13) 



In our case, however, there is a mechanical limit range for the elongation of each chamber and 
the corresponding pressure applied into the chamber of the Colobot. The objective function is 
constructed to be included in the inverse Jacobian algorithms as the second criteria also called 
the null-space method Hollerbach & Suh (1986); Nakamura (1991). 



Q p = J+X + F {I-J+J]g 



(14) 



where I is the identity matrix, ]i is constant and g is a second criterion for the optimization 
of the solution. This objective function evaluates the pressure difference between the applied 
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pressure in the chamber and the average pressure applied in the chamber. So the cost function 
w(P) is expressed as follows: 



- 1 L mve 



»M = 3 E (p^p. 

° i— i \ L vmax L imin 



(15) 



dw dw dw 
WidP^dPj, 



(16) 



We can then minimize w(p) by choosing: 

g = grad(w(P)) : 

6. Experimental results 

This section will present the implementation of the whole system and experimental results of 
automatic guidance capability of this system in a colon-like tube. 

6.1 Hardware implementation 

Fig. 24 shows the low-level control system of ColoBot. The pressurized air comes through 
the compressor (1) and the general pressure is adjusted thanks to the device (2). The pressure 
in the chambers are controlled by three Jet-pipe servovalves (3a, 3b and 3c). Three pressure 
sensors (4a, 4b and 4c) are connected between the servovalves and the Colobot (5) for the 
pressure feedback control. Suitable drivers and amplifiers in the rack (6) were designed to 
amplify control signals applied to the actuator. A real-time controller is implemented through 
a DSpace board and coupled with the Real-Time Workshop of Simulink. The Simulink block 
diagram designed for path planning and kinematics algorithms are expressed with Simulink 
block diagram which will be compiled as real-time executable under the DSP Processor of the 
DSpace board. The system runs at 500 Hz for a real-time control loop. 
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Fig. 24. The implementation of the whole system 



22 



Advances in Robot Manipulators 



6.2 Experimental results in a colon-like tube 

A more realistic experiment to test the performance of this semi-autonomous colonoscopy 
system is to use a colon-like transparent tube to see if Colobot can cross the tube with minimal 
contact with the tube wall. The diameter of the tube is 26 mm and its length is 50 cm (Fig. 25). 
For this guidance experiment, the calibration of the optical fibres was adapted to the transpar- 
ent tube. It is highly probable that results for the distance sensors in a porcine intestine will 
be similar to those obtained in the human bowel. However, the locomotion of the system is 
manually operated. The evolution of the measurements of three optical fibres are represented 
in the left Fig. 26(a). During the entire movement, the distances are never less than 0.8 mm. 
This demonstrates that the colonoscope tip is moving through the tube without touching it. 
For a better representation and visualization, Fig. 26(b) shows the extreme positions of the 
top-end of Colobot as it progresses (with a velocity of 4 cm/s). The position of the Colobot at 
the centre of the tube is represented by the smallest circle. The larger circle represents the tube 
wall and the line shows the extreme positions of Colobot. This experiment demonstrates that 
Colobot has the capability to guide the exploration of the tube with a sensor-based steering 
control method. 




Fig. 25. Guidance control test in a colon-like tube 



7. CONCLUSIONS AND FUTURE WORKS 

This paper presented a complete robotic system for semi-autonomous colonoscopy. It is com- 
posed of a microtip, a proximity multi-sensor system and high level real-time control system 
for guidance control of this robot. This system was focused on its guidance ability of endo- 
scope inside the human colon with the fiber optic proximity sensors. Colobot is a continuum 
robot made of silicone rubber. It has three DoF with its outer diameter of 17mm and the 
weight of 20 gram. The pneumatic actuators of ColoBot are independently driven through 
three servovalves. The kinematic model of this soft robot was developed based on the geomet- 
ric deformation and validated its correction. A method using a circumscribed circle is utilized 
to calculate the safe reference position and orientation of the Colobot. While kinematic-based 
orientation control used these reference paths to adjust the position of Colobot inside the colon 
to achieve guidance. Experimental results of guidance control with a transparent tube veri- 
fied the effectivity of kinematic control and guidance control strategy. In the near future, the 
proposed method will be tested in a vitro environment. 
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Evolution of measurements of three sensors 
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Abstract 

An adaptive controller is presented in this paper to control an n-link flexible-joint 
manipulator with time-varying uncertainties. The function approximation technique (FAT) 
is utilized to represent time-varying uncertainties in some finite combinations of orthogonal 
basis. The tedious computation of the regressor matrix needed in traditional adaptive 
control is avoided in the new design, and the controller does not require the variation 
bounds of time-varying uncertainties needed in traditional robust control. In addition, the 
joint acceleration is not needed in the controller realization. Via the Lyapunov-like stability 
theory, adaptive update laws are derived to give convergence of the output tracking error. 
Moreover, the upper bounds of tracking errors in the transient state are also derived. A 2 
DOF planar manipulator with flexible joints is used in the computer simulation to verify the 
effectiveness of the proposed controller. 

Keywords: Adaptive control; Flexible-joint robot; FAT 

1. INTRODUCTION 

In practical applications, most controllers for robot manipulators equipped with harmonic 
devices are based on rigid-body dynamics formulation. To achieve high precision tracking 
performance, the joint flexibility should be carefully considered. 1 However, the modeling of 
flexible-joint robots is far more complex than that of rigid-joint robots. Besides, the 
mathematical model of the robot inevitably contains model inaccuracies such as parametric 
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uncertainties, and unmodeled dynamics. Since these inaccuracies may degrade the 
performance of the closed-loop system, any practical design should consider their effects. 
Under the problems of joint flexibility and model inaccuracies, several strategies based on 
adaptive control or robust control for flexible-joint robots had been proposed. 
Spong 2 - 3 proposed an adaptive controller for flexible-joint robots by using the singular 
perturbation formulation. Chen and Fu 4 presented a two-stage adaptive control scheme for a 
single-link robot based on a simplified dynamic model. Khorasani 5 designed an adaptive 
controller using the concept of integral manifolds for n-link flexible-joint robots. Without 
using the velocity measurements, Lim et. al. 6 proposed an adaptive integrator backstepping 
scheme for rigid-link flexible-joint robots. Dixon et. al. 7 designed an adaptive partial state 
feedback controller by using a nonlinear link velocity filter. Yim 8 suggested an output 
feedback adaptive controller based on the backstepping design. Kozlowski and Sauer 9 - 10 
suggested an adaptive controller under the assumption of bounded disturbances to have 
semiglobal convergence. Tian and Goldenberg 11 proposed a robust adaptive controller with 
joint torque feedback. Jain and Khorrami 12 suggested a robust adaptive control for a class of 
flexible-joint robots that are transformable to a special strict feedback form. However, like 
most adaptive control strategies, the uncertainties should be linearly parameterizable into 
regressor form 13 . Availability of the regressor matrix is crucial to the derivation of adaptive 
controllers for robot manipulators. This is because traditional adaptive control strategies 
have a common assumption that the uncertain parameters should be constant or slowly time 
varying. Therefore, the robot dynamics is linearly parameterized into known regressor 
matrix and an unknown vector with constant parameters. In general, derivation of the 
regressor matrix for a given robot is tedious. Once it is obtained, we may find that, for most 
robots, elements in the unknown vector are simple combinations of system parameters such 
as link mass, link length and moment of inertia, and these are sometimes relatively easy to 
measure. 13 

Huang and Chen 14 proposed an adaptive backstepping-like controller based on FAT 15-28 for 
single-link flexible-joint robots with mismatched uncertainties. Similar to most backstepping 
designs, the derivation is too complex to robots with more joints. In this paper, we would 
like to propose a FAT based adaptive controller for n-link flexible-joint robots. The tedious 
computation of the regressor matrix is avoided in the new design. Moreover, the novel 
controller does not require the variation bounds of time-varying uncertainties needed in 
traditional robust control. In addition, the control strategy does not need to feedback joint 
acceleration. Convergence of the output error and the boundedness of all signals are proved 
using Lyapunov-like direct method with consideration of the effect of the approximation 
error. 

This paper is organized as follows: in section 2, we derive the proposed adaptive controller 
in detail; section 3 presents simulation results of a 2-D flexible-joint robot using the 
proposed controller; finally, some conclusions are given in section 4. 

2. MAIN RESULTS 

The dynamics of an n-rigid link flexible-joint robot can be described by 29 

D(q)q + C(q, q)q + g(q) = K(8 - q) (1) 
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J6 + B6 + K(0 - q) = u (2) 

where (J G 9i is the vector of link angles, 6 £= V\ is the vector of actuator angles, 

U £ 9t is the vector of actuator input torques, D((j) is the n x n inertia matrix, 
C(q, (j)q is an n-vector of centrifugal and Coriolis forces, and g(q) is the gravity vector. 

J , B and K are n x n constant diagonal matrices of actuator inertias, damping and 
joint stiffness, respectively. Here, we would like to consider the case when the precise forms 
of D(q), C(q, q)q and g(q) are not available and their variation bounds are not given. 

This implies that traditional adaptive control and robust control cannot be applicable. In the 
following, we would like to use the function approximation technique to design an adaptive 
controller for the flexible-joint robot. Moreover, it is well-known that derivation of the 
regressor matrix for the adaptive control of high DOF rigid robot is generally tedious. For 
the flexible-joint robot in (1) and (2), its dynamics is much more complex than that of its 
rigid-joint counterpart. Therefore, the computation of the regressor matrix becomes 
extremely difficult. Different form the conventional adaptive control schemes for robot 
manipulators, the proposed FAT-based adaptive controller does not need the computation 
of the regressor matrix. This largely simplifies the implementation of the control loop. 

Define T = K(6 - q) to be the vector of transmission torques, so (1) and (2) becomes 11 

D(q)q + C(q,q)q + g(q) = T (3) 

J,T + B r x + T = u-q(q,q) (4) 

where 3 t = JK , B, = BK and q(q,q) = Jq + Bq. Define signal vector 

S = e + Ae and V = q^ — Ae, where q^ G 91" is the vector of desired states, 

e = q-q rf is the state error, and A = diag(A l , X 2 ,..., X n ) with X t > for all 
i=l, ... n. Rewrite (3) in the form 

Ds + Cs + g + Dv + Cv = t (5) 

A. Controller Design for Known Robot 

Suppose D(q) , C(q, q)q and g(q) are known, and we may design a proper control law 

such that T follows the trajectory below 

T = g + Dv + Cv-K rf s (6) 

where K^ is a positive definite matrix. Substituting (6) into (5), the closed loop dynamics 
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1 T 
becomes Ds + Cs + K^S = 0. Define a Lyapunov function candidate as V = — S Ds. 

Its time derivative along the trajectory of the closed loop dynamics can be computed as 
V = — S K rf S + S (D — 2C)s. Since D — 2C can be proved to be skew-symmetric, the 

above equation becomes V = — S K^S < 0. It is easy to prove that s is uniformly bounded 

and square integrable, and S is also uniformly bounded. Hence, S — > as t — > oo , or 
we may say e — > as I — > oo . To make the actual T converge to the perfect T in (6), 
let us consider the reference model 



J,T, +B,.T,. +K,T r =K,.T, +B r t rf + J,.T, 



(7) 



where T e5{ is the state vector of the reference model and T d £ Zn is the desired 

states. Matrices J £ Vx , B G VX and K £ VX are selected such that 

7 r —>T d exponentially. Define T r/ (T £/ ,T^)=K / . (B ( .T^ +J / .T rf ), we may rewrite (4) and (7) 
in the state space form as 



A A +B P uB ,q 



(8) 



% n =KK+^nk\i+^d) 



(9) 



where X p = [t t] G 9? 2 " and X m = [t,. T,. ] S 9l 2 " are augmented 

I_ 



vectors. 

A,„ = 
B„ 



-Jr 1 -j;b ; 



9? 



2/1x2/1 



state 



and 



j;k, -j;B r 



9? 



2nx2n 



are augmented system matrices . 



o 
J 1 



ft 2nxn and B. 







» 2/7X/7 



91 are augmented input gain 



_j;k, 

matrices, and the pair (A , B ) is controllable. Since all system parameters are assumed to 
be available at the present stage, we may select a controller in the form 30 



u = 0s +Or^+h(T (/ ,q) 



(10) 



> nx2n 



where e v\" XA " and O € 9T*" satisfy A + B „0 = A„ and B O = B m , 
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respectively, and h(T rf ,q) = OT rf + (J . Substituting (10) into (8) and after some 
rearrangements, we may have the system dynamics 

X P =A,^+B,„(T,+T,) (11) 

Define C = X — X m and we may have the error dynamics directly from (9) and (11) 

e,„ = A m e„ (12) 

Let C = T — T be the output vector of the error dynamics (12) as 

e =C e (13) 

r mm v ' 

where C £ 9i is the augmented output matrix such that the pair (A _,C ) is 

observable and the transfer function C (jSI — A ) B m is strictly positive real. Since 
A is stable, (12) implies G — >0 as t — > oo . This further gives T — ^T^ as 

B. Controller Design for Uncertain Robot 

Suppose D((j) , C(q, q)q and g(q) are not available, and (j is not easy to measure, we 

would like to design a desired transmission torque T d so that a proper controller u can be 

constructed to have T — > T d . 

Instead of (6), let us design a desired transmission torque T d as 

T d =g + Dv + Cv-K^s (14) 

where K rf > jl„ x „, and D, C and g are estimates of D(q), C(q, q) and g(q), 
respectively. Substituting (14) into (5), we may have the closed loop dynamics 

Ds + Cs + K J s = (T-T d ) + (D-D)v + (C-C)v + (g-g) (15) 

If a proper controller u and update laws for D , C and g can be designed, we may have 

T — ^T rf , D — > D, C — > C and g — > g so that (15) can give desired performance. Let 
us consider the control law 
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U = &L p +Ot d +h (16) 

where h is an estimate of h . Substituting (16) into (8), we may have the system dynamics 
*„ = A m x p + B„, (T rf + T d ) + B p (h - h) (17) 

Together with (9), we may have the error dynamics 

e„, =A IB e M +B / ,(h-h) (18) 

e r =C„,e„, (19) 

If we may design an appropriate update law such that h — > h , then (18) implies 

& m — > as t — > CO . This further implies T — > T d as t — > co . Since D, C, g and h are 

functions of time, traditional adaptive controllers are not directly applicable. To design the 
update laws, let us apply the function approximation representation 15 " 21 

D = W D r Z D +£ D , C = W c r Z c +£ c , 

(20a) 

g=w g r z g+£g , h=w;z h+£h 

where W D G ST^*", W c e SK"** 5 *" , W g sW« X ", and W h S W fi >™ are 

weighting matrices, Z D e W^™ Z c £ 9T VcX " , Z g S St"^" 1 , and Z h t= JR'A* are 

matrices of basis functions, and E^are approximation error matrices. The number p.. 

represents the number of basis functions used. Using the same set of basis functions, the 
corresponding estimates can also be represented as 

D = W D Z D , C = W c r Z c , 

g = W g r Z g , h = W h r Z h 

Define W () = W () — W () , then equation (15) and (18) becomes 

Ds + Cs + K^s = (t - r d )- W„Z D v - W c r Z c v - W g r Z g + s, (21) 
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e,„=A„,e ffl -B p W h rZ h+B,£ 2 (22) 

where *"l — t 'iv i 'D'*'C'*'g'''4a'7 and ^2 — *'2\*'h> m) are lumped approximation 
errors. Since W, * are constant vectors, their update laws can be easily found by proper 
selection of the Lyapunov-like function. Let us consider a candidate 

F( S ,e m ,W D ,W c ,W g ,WJ=VD S +<P f e m 

(23) 

+ -7>(W D r Q D W D +W c r Q c W c + W g r Q g W g +W h r Q h W h ) 

where P ( = P ( £ 91 is a positive definite matrix satisfying the Lyapunov equation 



A J m P,+P ( A ffl =-C:C„, The matrices Q D 6 *» *~ *, Q c 6 9f *~ *, 

Q g e9?" /?sX " /?s and Q h G SH" A>< " A are positive definite. The notation 7>(.) denotes the 

trace operation of matrices. The time derivative of V along the trajectory of (21) and (22) can 
be computed as 

F = s r Ds + i S r i)s + eI ! P,e m +e:P,e ; „ 

-7>(W D r Q> D + W C Q> C + W g r Q g \V g +W h r Q> h ) 
= -s r K,s + s r e r - e T r e r + s r £, + e^P,B p c 2 (24) 

-7r[W D r (Z D vs r +Q> D ) + W c r (Z c vs r +Q C W C )] 
-r^W g r (Z g s r + Q g W g ) + W h r (Z h e^B p +Q>J] 

According to the Kalman-Yakubovic Lemma, we have e n) P ( B = G T by 
picking B m = B 31 . According to (24), the update laws can be selected as 



W D = -Q D 1 Z D vs r -<r D W D , W c = Q c Z c vs r -<r c W c , 

^ g = -Q g 'z/ - <x g W g , \V h = -Q" 1 ^ " ^ w h 

where &,. are positive numbers. Then (24) becomes 



(25) 



34 



Advances in Robot Manipulators 



V = -[s T <]Q 



+ [s r eH 



+ <7 D 7>(W D r W D ) 



(26) 



+ <7 c 7>(W c r W c ) + tT E rr(W5 W ) + <7 h 7>( W h r W h ) 



where Q 



-I. 



"^ 



is positive definite due to proper selections of K f 



and K e . Owing to the existence of £ l and £ 2 the definiteness of V cannot be 

determined. According to Appendix Lemma A.lLemma A.4 and Lemma A. 7, the right hand 
side of (26) can be divided into two parts to derive following inequalities 



-[s r <]Q S + [s t e T T ] 



< — 



1 



^ m ,„(Q) 



v 



l 



WQ) 



2^ 



7>(W D r W D ) < |rr(W D r W D )-|rr(W D r W D ) 

7>(W c r W c ) < ^Tr(W T c W c )-^Tr(W T c W c ) 

rKw;w g )4rKw;w g) -Ir K wJw g ) 

rr(W h r W h )<|rr(W h r W h )-|rr(W h r W h ) 



(27a) 

(27b) 
(27c) 
(27d) 
(27e) 



According to (23), we hav 

F = i[ S r Ds + e:,P,e„, + 7>(W D r Q D W D +W c r Q c W c + W g r Q g W g + W h r Q h W h )] 



< 



1 



^ m ax(A) 



where A 



D 







2C PC 



+ ^ax(Q D )^(W D r W D ) + l max (Q c )rr(W c r W c ) 
+ ^x(Q g )7>(W g r W g ) + ^ max (Q h Fr(W h r W h )] 

. With (27) and (28), (26) can be further written as 



(28) 
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V < -aV + - 



1 



M ma x(A)-A™„(Q)] 



+ M m ax(Q D )-^ D FKW D r w D ) 



+ M m ax(Qc)-^cFKW c r w c ) + M max (Qj-a g Fr(w g r w g ) 



(29) 



+ M m ax(Qh)-^FKW h r w h ) + 



l 



4™(Q) 

+ <7 D 7r(W D r W D ) + a c 7>(W c r W c ) + <r g 2>(W g r W g ) + <x h 7V(W h r W h )} 

Although D and L are unknown, we know that 3 D and D s.t. D < D < D 

3landZ s.t. Z<|L|<Z 3fj A ,rj A >0 s.t.A max (A) < rj A and /L mm (A)>?/ 



no 32. 



Picking a < min- 



4nm(Q) 



we have 



F<-aF + 



1 



2/1 . (Q) 

mm V ^>c / 



^x(Qd) 4,ax(Q C ) 4,ax(Q S ) ^ax(Qh) 



+ -Krr(w D r w D ) 



then 



(30) 



+ <7 c 7r(W c r W c ) + <x g 7r(W g r W g ) + <7 h 7r(W h r W h )] 

Hence, V < whenever 

(s,e r ,W D ,W c ,W g ,W h )e{(s,e r ,W D ,W c ,W g ,W h )|K> 

2 



1 

2a 



1 
sup 

^(Q)4 



8 2 (Y) 



+ <7 D 7>(W D r W D ) 



+ <7 c 7>(W c r W c ) + <x g 7>(w g r w g ) + <7 h 7>(w;w h )]} 

This further concludes that s, e r , ©. , ^%j , W c , W , and W h are uniformly 

ultimately bounded(w.w.b.). The implementation of the desired transmission torque (14), 

control input (16) and update law (25) does not need to calculate the regressor matrix which 

is required in most adaptive designs for robot manipulators. The convergence of the 

parameters, however, can be proved to depend on the persistent excitation condition of the 

input. 

The above derivation only demonstrates the boundedness of the closed loop system, but in 

practical applications the transient performance is also of great importance. For further 
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development, we may apply the comparison lemma 32 to (30) to have the upper bound for V 

as 



a('-'o)l 



V(t)<e- aK '- l °'V(t ) + 



1 

2a 



1 



sup 

^min(Q)'o< r <' 



£l(0 

e 2 (t) 



+ <7 D 7V(W D r W D ) 



(31) 



+ <7 c 7r(W c r W c ) + cr g rr(W g r WJ + <x h 7>(W h r W h )] 



From (23), we obtain 
1 



v>- 



-U(A) 



+ ^„(Q D )7>(w D r w D ) + A m „(Qc)^(w c r w c ) 



(32) 



+ ^(QjrrCWfWJ + A min (Q h )rr(W h r W h )] 



Thus, the bound of 



Ik <J 



for t > t can be derived from (31) and (32) as 



^— [V-^(Q»)Tr(W*W B )-A^(Q c )Tr(W T c W c ) 



n 



^ min (Q g )^(W g r W g )-^ min (Q h )rr(W 1 fW h ) 



'/ 



2 e - a «- t ° , V(t )- 



a 



1 



sup 

^ min (Q)'o<^<' 



£ ; (r) 
s 2 (t) 



<x D 7V(W D r W D )( 33 ) 



+ a c 7V(W c r W c ) + <x g 7V(W g r W g ) + a h 7>(W h r W h )] 

-^,„(Q D )^(W D r W D )-l min (Q c )rr(W c r W c ) 

-^,„(Q g )^(W g r W g )-^ min (Q h )rr(W h r W h )} 



From the derivations above, we can conclude that the proposed design is able to give 
bounded tracking with guaranteed transient performance. The following theorem is a 
summary of the above results. 

Tlieorem 1; Consider the n-rigid link flexible-joint robot (1) and (2) with unknown parameters 
D, C, and g then desired transmission torque (14), control input (16) and update law (25) 
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ensure that 

(i) error signals s, C T , W D , W c , W , and W h are u.u.b. 

(ii) the bound of the tracking error vectors for t > t Q can be derived as the form of (33), if 
the Lyapunov-like function candidates are chosen as (23). 

Remark 1: The term with (7, , in (25) is to modify the update law to robust the closed-loop 
system for the effect of the approximation error 17 . Suppose a sufficient number of basis 
functions p , < is selected so that the approximation error can be neglected then we may 

have <T () = , and (26) becomes 

r S 



V = -W ef]Q 



< (34) 



It is easy to prove that s and G T are also square integrable. From (21) and (22), S and e r 

are bounded; as a result, asymptotic convergence of s and e r can easily be shown by 

Barbalat's lemma. This further implies that T — > T d and (J — > (\ d even though D, C, 

and g are all unknown. 

Remark 2: Suppose 8i and 62 cannot be ignored but their variation bounds are available 16 - 17 i.e. 

there exists positive constants Si and Si such that £j < Oj, and £ 2 — u 2 ■ To cover the 

effect of these bounded approximation errors, the desired transmission torque (14) and the 
control input (16) are modified to be 

T d = DV + CV + g - K rf S + T mbmn (35) 

U = &X p +®T ld +h + T mbust2 (36) 



where tmbusti and twbusa are robust terms to be designed. Let us consider the Lyapunov-like 
function candidate (23) and the update law (25) again. The time derivative of V can be 
computed as 



V = -V ef]Q 



s 
e. 



+ £ 1 |s||+£je J| + s r T rotol + efT roto2 (37) 



By picking T robusn =-5 x [sgn{s x ) ••• Sgn(s„)] , where s, ( , k=l,...,n is the fc-th 
element of s, and T robust2 =-£ 2 [sgn(<? ) ••• Sgn(e r )f where e , k=l,...,2n 



is 



the /c-th element of e„ we may have V ^ , and asymptotic convergence of the state error 
can be concluded by Barbalat's lemma. 
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3. SIMULATION STUDY 

Consider a planar robot (Fig.l) with two rigid links and two flexible joints represented by 
the differential equation (1), and (2). The quantities nij, U, Id and I, are mass, length, gravity 
center distance and inertia of link i, respectively. Actual values of link parameters in the 
simulation are 18 mi=0.5kg, m2=0.5kg, h=h=0.75m, 1 c i=1 C 2 = 0375m, h=0.09375kg-m 2 , and 
l2=0.04:6975kg-m z . The actuator inertias, damping, and joint stiffness are 

J = diag (0. 02,0. 0\)(kg • m 2 ), B = diag(5,4)(Nm ■ sec/ rad) and 

K = diag(\00,\00)(Nm / rad) respectively. We would like the end-point to track a 

0.2m-radius circle centered at (0.8 in, 1.0 in) in 10 seconds. To have more challenge, we pick 
the initial condition of the link angles and the motor angles as 

q = [-0.184 1.94 Of and 6 = [-0.184 1.94 0] T that are 
significantly away from the desired trajectory. The initial value of the reference model state 
vector is T r = 10.39 -0.72 01 which is the same as the initial value of the 
desired reference input T d . The controller gains are selected as K rf = diag(0. 1,0.1) 

and A = diag(5,5). Each element of D, C, g and h is approximated by the first 41 

terms of the Fourier series. The simulation results are shown in Fig. 2 to 8. Fig. 2 shows the 
tracking performance of the end-point and the desired trajectory in the Cartesian space. It is 
observed that the end-point trajectory converges nicely to the desired trajectory, although 
the initial position error is quite large. Fig. 3 is the joint space tracking performance. It shows 
that the transient response vanishes very quickly. Fig. 4 is the actuator inputs in N-m. Fig. 5 
to 8 are the performance of function approximation for D, C, g and h respectively. Since the 
reference input does not satisfy the persistent excitation condition, some estimates do not 
converge to their actual values but remain bounded as desired. It is worth to note that in 
designing the controller we do not need much knowledge for the system. All we have to do 
is to pick some controller parameters and some initial weighting matrices. 

4. CONCULSIONS 

In this paper, we have proposed a FAT-based adaptive controller for a flexible joint robot 
containing time-varying uncertainties. The new design is free from regressor calculation and 
knowledge of bounds of uncertainties. 

Feedback of the joint acceleration is also avoided. The function approximation technique is 
used to deal with time-varying uncertainties. Using the Lyapunov like analysis, rigorous 
proof of the closed loop stability has been investigated with consideration of the 
approximation error. Computer simulation results justify its feasibility of giving satisfactory 
tracking performance on a 2-D flexible-joint robot although we do not know much 
knowledge about the system model. 
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Fig. 1. 2-DOF planar robot 




Fig. 2. Tracking performance of end-point in the Cartesian space ( actual; — desired). 
Initial position of end-point is at the point (0.6m, 0.6m). After some transient, the tracking 
error is very small, although we do not know precise dynamics of the robot. 
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Fig. 3. The joint space tracking performance( actual; — desired). The real trajectory 
converges very quickly. 
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Fig. 4. Actuator input. 
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Fig. 5. Approximation of D matrix( estimate; — real). Although the estimated values do 
not converge to the true values, they are bounded and small. 
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Fig. 7. Approximation of vector g( estimate; — real). 
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APPENDIX 

Lemma A.l: 

Let S £ 9t , £ 6 9v and K is the n x n positive definite matrix. Then, 

II II 2 

- s r Ks + s r £ < \[A mm (K)||s|| 2 - JL] . (A.i) 

Proof: 
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Q.E.D. 



Lemma A.l: 

Let W- = [Wj W. 2 ••" W. ] £ 9i , i=l,...,m and W is a block diagonal matrix 

defined as W = Jmg{w l5 W 2 , • • • , W„, } 6 9T" Xm . Then, 

rr(W r W) = ^|w.| (A.2) 



/=! 



The notation Tr(.) denotes the trace operation. 
Proof: The proof is straightforward as below: 
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Lemma A3: 
Suppose 

< = [v„ v ;2 • 
are defined 
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V = diag{\ x , V 2 , • • • , V m } G % mn * m t respectively. Then, 
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Proof: The proof is also straightforward: 
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(A.3) 



Lemma A.4: 

Let W be defined as in Lemma A.2, and W is a matrix defined as W = W — W , 
where W is a matrix with proper dimension. Then 



J>(W r W) < -7>(W r W) - -Tr(W T W) . 



(A.4) 



Proof: 
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Tr(W T W) = Tr(W T W) - 7>(W r W) 
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Q.E.D. 
In the above lemmas, we consider properties of a block diagonal matrix. In the 
following, we would like to extend the analysis to a class of more general matrices. 

Lemma A.5: 

Let W be a matrix in the form W 7 = [W^ W 2 r ••• Wj] G SK"™""" where 

W, = «Wg'{W ; j,W ; . 2 ,-",W ;m } £ 9? , i=l,...,p, are block diagonal matrices with the 

entries of vectors W l; = [w tl W ij2 •■■ W //n ]e9i ,j=l,...,m. Then, we may have 
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Lemma A.6: 

Let V and W be matrices defined in Lemma A.5, Then, 
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Lemma A.7: 

Let W be defined as in Lemma A.5, and W is a matrix defined as W = W — W , 
where W is a matrix with proper dimension. Then 

Tr(W T W)<-Tr(W T W)--Tr(W T W). (A.7) 

Proof: 

7>(W r W) = Tr(W T W) - 7>(W r W) 
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1. Introduction 

A wheelchair-mounted robotic arm (WMRA) system was designed and built to meet the 
needs of mobility-impaired persons with limitations of upper extremities, and to exceed the 
capabilities of current devices of this type. The control of this 9-DoF system expands on the 
conventional control methods and combines the 7-DoF robotic arm control with the 2-DoF 
power wheelchair control. The 3-degrees of redundancy are optimized to effectively 
perform activities of daily living (ADLs) and combine wheelchair mobility and arm 
manipulation to overcome singularities, joint limits and some workspace limitations. The 
control system is designed for teleoperated or autonomous coordinated Cartesian control, 
and it offers expandability for future research. Several interchangeable user interfaces were 
implemented in the design, including a Brain Computer Interface (BCI). That BCI system 
was modified and integrated to the control of the WMRA system for users who are totally 
paralyzed or "locked-in" and cannot use conventional augmentative technologies, all of 
which require some measure of muscle control. Testing and data collection were performed 
on human subjects, and the design, various optimized control methods and test results are 
presented in this paper. 

According to the 2006 US Census Bureau report (US Census Bureau, 2002), about 51.2 
million Americans (18.1 percent of the US population) had some level of disability and 32.5 
million of them (11.5 percent) had a severe disability. About 10.7 million Americans older 
than 6 years of age needed personal assistance with one or more activities of daily living 
(ADL). This work focuses on people who have limited or no upper extremity mobility due 
to spinal cord injury or dysfunction, or genetic predispositions, or people who are "locked- 
in" (e.g., by end-stage amyotrophic lateral sclerosis, brainstem stroke, or severe 
polyneuropathy). Robotic aides used in these applications may vary from advanced limb 
orthosis to robotic arms (Reswick, 1990). 

A wheelchair mounted robotic arm can enhance the manipulation capabilities of individuals 
with disabilities that are using power wheelchairs, and reduce dependence on human aides. 
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Unfortunately, most WMRAs have had limited commercial success due to poor usability 
and low payload. It is often difficult to accomplish many of the Activities of Daily Living 
(ADL) tasks with the two commercial WMRAs currently on the market due to its physical 
and control limitations. Furthermore, the lack of the integration of the robotic arm controller 
with the wheelchair controller leads to an increased mental load on the user. This project 
attempts to surpass available commercial WMRA devices by offering an intelligent system 
that combines the mobility of the wheelchair and the manipulation of a newly designed arm 
in an effort to improve performance, usability, control and reduce mental burden on the 
user while maintaining cost competitiveness. 

It is desired to fulfill the need of such integrated systems to be used for many ADL tasks 
such as opening a spring-loaded door autonomously and going through it, interactively 
exchange objects with a companion on the move, and avoiding singularities in a small 
working environment, such as an office, where wheelchair motion can be slightly utilized to 
maneuver objects while avoiding singularities (similar to a person sitting on an office chair 
and handling surrounding objects by moving his/her arm while slightly moving the chair to 
get closer to an object that is otherwise unreachable). These tasks can be performed without 
the need to switch between the wheelchair controller and the robotic arm controller. The 
implementation of the combined control will still give the user the option to control the 
robotic arm alone or the wheelchair alone. 

In addition, a Brain-Computer Interface (BCI), which does not require any muscular activity 
for device manipulation, is also a feasible control mechanism for the totally paralyzed users. 
For a "locked-in" user, the BCI control system introduces a wide range of self -performed 
ADL tasks that are otherwise impossible to perform independently. 

2. Background 

There are several designs of workstation-based robotic arm systems, but WMRAs combine 
the idea of workstation and mobile-base robots to mount a manipulator arm onto a power 
wheelchair. One of the most important design considerations of where to mount a robotic 
arm in a power wheelchair is the safety of the operator (Yanco, 1998). There have been 
several attempts in the past to create commercially viable wheelchair mounted robotic arms, 
including the two currently available WMRAs: Manus and Raptor. 

The Manus manipulator, manufactured by Exact Dynamics, available since the early 1990s 
(Eftring & Boschian, 1999), can be programmed in a manner comparable to industrial robotic 
manipulators. A picture of Manus mounted onto a wheelchair is shown in Figure 1. It is a 6 
DoF arm, with servomotors all housed in a cylindrical base. Besides the fact that it is 
controlled independent of the wheelchair control, the current controller allows for Cartesian 
control, but when it comes close to a singularity, it stops and waits for the user to move it in 
a different direction. This kind of control increases the cognitive load on the user. 
Another production WMRA is the Raptor, manufactured by Applied Resources (Mahoney, 
2001), as shown in Figure 2, which mounts to the right side of the wheelchair. This 
manipulator has four degrees of freedom plus a planar gripper. The user directly controls 
the arm with either a joystick or a keypad controller. Because the Raptor does not have 
encoders, the manipulator cannot be controlled in Cartesian coordinates. This compromise 
was done to minimize the cost, but it decreases the usability of the arm. 
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Various redundancy resolution methods were developed in the past to optimize a solution 
based on certain criteria function. Weighted least norm solution method was used by Chan 
et al (Chan & Dubey, 1995) to penalize the motion of some joints over others. This method 
can be used in the case of WMRAs to make the wheelchair motion as a secondary motion 
when needed. The combination of mobility and manipulation has been studied by 
researchers in the form of a mobile platform that carries a robotic arm. Chung, et al (Chung 
& Velinsky, 1999) resolved the kinematic redundancy by decomposing the mobile 
manipulator into two different subsystems, the mobile platform and the manipulator. Each 
one of these subsystems is controlled independently with an interaction algorithm between 
the two controllers. 




Fig. 1. Manus arm. 




Fig. 2. Raptor arm. 



Mirosaw (Galicki, 2005) used external penalty functions to enforce the holonomic 
manipulability and collision avoidance. His results showed continuous velocities near 
obstacles. An extension to different redundancy resolution schemes has been proposed by 
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Luca (Luca et al., 2006) to include the representation of mobile platforms in the Jacobian. His 
simulation showed consistent results. 

BCI systems have been used in the past to control peripheral devices such as TVs, cell 
phones, and computers among others (Farwell & Donchin, 1988). In the case of BCI- 
controlled computers, the control can be extended to operate rehabilitation devices, 
prosthetic limbs, and robotic hands. However, using a brain computer interface to control a 
robotic system is relatively new and primarily at a research stage. In this work, conventional 
user interfaces, such as a spaceball, a joystick, a keypad and a touch screen, were used to 
control the WMRA system. A Brain-Computer Interface (BCI) was modified along this line 
of work and implemented as one of the modular interchangeable user interfaces for the 
WMRA to be controlled and used by the locked-in patients who are paralyzed from the neck 
down. This allows the user to communicate action choices to the robot using the BCI. 
Farwell and Donchin (Farwell & Donchin, 1988) developed a BCI system that utilized the 
P300 component of the Event Related Brain Potential (ERP) to allow a locked-in patient to 
"type" text into a computer without using any neuromuscular function. 

D. Valbuena et. al. and T. Liith et. al. (Valbuena et al., 2007; Liith et al, 2007) used a Steady- 
state Visual Evoked Potentials (SSVEP) system to control a semi-autonomous robot to 
provide a user IV2 hours of independence. The BCI transforms high level orders from the 
user into low level commands for the robot. The user selects tasks, such as pouring a liquid 
into a glass, from a menu to control the robotic arm. 

The main objective of this work is to develop and optimize a control system that combines 
the manipulation of the newly designed 7-DoF robotic arm and the mobility of a modified 2- 
DoF wheelchair in a single control algorithm. Redundancy resolution is to be optimally 
solved to avoid singularities and joint limits as well as to allow larger wheelchair or 
manipulator motion depending on the proximity to the goal. The controller is capable of 
moving autonomously or using teleoperation. A Brain-Computer Interface (BCI) system is 
to be modified and adapted to the modular control algorithm to allow locked-in users to 
communicate with the WMRA system and command it to perform a pre-selected list of 
commands. 



3. Motion Control of the 9-DoF WMRA System 

3.1 Wheelchair Motion 

The differential drive used in power wheelchairs represents a 2-DoF system that moves in 
plane (Papadopoulos & Poulakakis, 2000). When wheelchair motion control is not combined 
with the mobile manipulator control, it is always desired to align the wheelchair in certain 
direction to position the arm in the desired position and orientation to perform ADLs. In 
this case, non-holonomic constraints on mobile platforms restrict the system's ability to 
control all 3-DoFs in the workspace. Trajectory planning is required to compensate for the 
lost DoF in that plane. Suppose that the wheelchair is commanded to move the arm's base 
reference frame from "TO" position to "XI" position, where "T" is the homogeneous 
transformation matrix of the corresponding configuration, the motion can be divided into 
three sub-motions that can be planned in six steps to realize the X-direction motion, Y- 
direction motion and the Z-direction orientation. The following six steps can be 
programmed to execute these three sub-motions: First, from the initial point of the arm base 
at "TO", find the corresponding wheelchair's frame transformation matrix at that pose. 
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Second, from the destination point of the arm base at "Tl", find the corresponding 
wheelchair's frame transformation matrix at that pose. Third, draw a virtual line between 
the two new frame transformations of the wheelchair's frame, and find the angle of that line 
using the transformation resultant between the two. Fourth, command the wheelchair to 
rotate to the angle of the new line with no translation. Fifth, command the wheelchair to 
move in a straight line from the initial position to the final position of the wheelchair, 
ignoring the orientation. Sixth, command the wheelchair to rotate from the angle of the new 
line to that of the final position. Figure 3 shows the trajectory planning resulting in three 
sub-motions when given the initial and final position and orientation: 




Fig. 3. Trajectory planning for planar motion. 

When the wheelchair control is combined with the manipulator control, the above 
procedure is not necessary since the total system will be redundant. Assuming that the 
manipulator is mounted on the wheelchair with "L2" and "L3" offset distances from the 
center of the differential drive across the x and y coordinates respectively (as shown in 
Figure 4), the mapping of the wheels' velocities to the manipulator's end effector velocities 
along its coordinates is defined by: 
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Fig. 4. Coordinate frames and dimensions of interest. 

where "P X g" and "Pyg" are the x-y coordinates of the end-effector based on the arm base 
frame, "O" is the angle of the arm base frame, which is the same as the angle of the 
wheelchair based on the ground frame, "Ls" is the wheels' radius, and "Lj" is the distance 
between the two driving wheels. The above Jacobian can be used to control the wheelchair 
with the Jacobian of the arm after combining them together. 

3.2 The 7-DoF Arm Motion 

From the D-H parameters specified in an earlier publication (Alqasemi et al., 2005), the 6x7 
Jacobian that relates the joint rates to the Cartesian speeds of the end effector based on the 
base frame is generated according to Craig's notation (Craig, 2003) 



r = J r V t 



(2) 



where: 
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r = |i: y Z a /? y\ is the task vector, and 
V A =[dj 6 2 3 4 9 i 4 7 J is the joint rate vector. 

Numerical solutions are implemented using the Jacobian to follow the user directional 
motion commands or to follow the desired trajectory. Manipulability measure (Yoshikawa, 
1990) is used as a factor to measure how far is the current configuration from singularity. 
This measure is defined as: 

w = jdet(J A *J T 4 ) (3) 

Redundancy is resolved in the program structure using Pseudo Inverse of the Jacobian 
(Yoshikawa, 1990), and singularity is avoided by maximizing the manipulability measure. 
Since this method carries a guaranteed valid solution only at a singular configuration and 
not around it, the results carried high joint velocities when singularity is approached. We 
then decided to use S-R Inverse of the Jacobian (Nakamura, 1991) to give a better 
approximation around singularities, and use the optimization for different subtasks. S-R 
Inverse of the Jacobian is used to carry out the inverse kinematics as follows: 

r A =j T A *{j A *j T A +k*i 6 r w 

where "h" is a 6x6 identity matrix, and "k" is a scale factor. It has been known that this 
method reduces the joint velocities near singularities, but compromises the accuracy of the 
solution by increasing the joint velocities error. Choosing the scale factor "k" is critical to 
minimizing the error. Since the point in using this factor is to give approximate solution near 
and at singularities, an adaptive scale factor is updated at every time step to put the proper 
factor as needed: 

k *(l-—) 2 for w<w (5) 




for w > w 

where "wo" is the manipulability measure at the start of the boundary chosen when 
singularity is approached, and "ko" is the scale factor at singularity. It was found that the 
optimum values of "wo" and "ko" for our system are 20xl0" 3 and 0.35xl0" 3 respectively. Now 
that the singularity is taken care of using the S-R Inverse of the Jacobian, we can use the joint 
redundancy to optimize for a secondary task as follows: 

V d =J* A *r d +(I 7 -J' A *J A )*f (6) 

where "f is a 7x1 vector representing the secondary task. That task can either be the desired 
trajectory in the case of pre-set task execution, or it can be a criterion function that represents 
the potential energy to be minimized. 
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3.3 The 9-DoF WMRA System Motion 

Combining the two subsystems together by means of Jacobian augmentation (Luca et al., 
2006) can give the flexibility of using conventional control and optimization methods 
without compromising the total system coordinated control. In the case of combined control, 
let the task vector be: 

r = f(q c ,q A (7) 



Differentiating (7) with respect to time gives: 

r = ^~V c+ ^-V A =J e J w V e+ J A r A = [jj Ja ] 

oq c oq A 



or, r = J • V 



(8) 



Solving (8) in conventional methods is now possible. Choosing the Projected Gradient 
method, gives: 

v ;}=rr + {i-rj)v p) 

' A 



where y = aV H(q) f° r conventional arms, and "H(q)" is the optimization criteria 

y = H(q). 

The existence of the mobile platform means that "Vo" may not exist for non holonomic 
constraint such as that of the wheelchair. To go around this limitation (Luca et al., 2006) 
proposed the following: Differentiate the optimization criteria function "H" with respect to 
time as follows: 



dq c ' dq A 
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In this case, the value of " VH" that improves the objective function is: 

V q H{q) = V 



V H =±a 
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(11) 



and that velocity vector can be used for optimization. This gives a good representation of 
the arm joints' velocities and the wheels' velocities of the wheelchair. 

Weighted Least Norm solution can also be used as proposed by (Chan & Dubey, 1995). In 
order to put a motion preference of one joint rather than the other (such as the wheelchair 
wheels and the arm joints), a weighted norm of the joint velocity vector can be defined as: 



■4v T wv 



(12) 



where "W" is a 9X9 symmetric and positive definite weighting matrix, and for simplicity, it 
can be a diagonal matrix that represent the motion preference of each joint of the system. For 
the purpose of analysis, the following transformations are introduced: 
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J w =JW- [n and V w = W-" 2 V (13) 

From (12) and (13), it can be shown that the weighted least norm solution is: 

V w =W- l J T (jW- l J T Yr (14) 

The above method has been used in simulation of the 9-DoF WMRA system with the nine 
state variables "Vd" that represent the seven joint velocities of the arm and the two wheels' 
velocities of the power wheelchair. It was found that the latter two state variables are of 
limited use since they tend to unnecessarily rotate the wheelchair back and forth during a 
long forward motion due to their equal weights. Changing the weights of these two 
variables will only result in a preference of one's motion over the other. 

Two new state variables were introduced out of the wheels' velocities, which represent the 
angular speed of the wheelchair when both wheels run at equal but opposite velocities and 
the forward speed of the wheelchair when both wheels run at equal velocities as follows: 

<j> = 2 ^, and X = L9 ( 15 ) 

I 

The combination of the above two variables would be sufficient to describe any forward and 
rotational motion of the wheelchair. Having these two state variables in vector "V" instead 
of the wheels' velocities give a greater advantage in controlling the preferred rotation or 
translation of the wheelchair. The wheelchair's Jacobian in (1) was changed for the new state 
variables before augmenting it to the arm's Jacobian, and the results were much better in 
terms of valuable control. The new state variables are: 

v = [e { e 2 e\ e 4 e, d 6 e\ x jf (i6) 

Care must be taken when implementing the above change in the controller algorithm since 
one of the state variables (the forward motion of the wheelchair) carry linear velocity units, 
which are different from the rest of the state variables, which carry angular velocity units. 

3.4 Criterion Function for Joint Limit Avoidance 

The criterion function used for optimization can be defined based on the physical joint limits 
of the WMRA system, and minimizing such a function results in limiting the joint motion to 
its limit. A mathematical representation of joint limits in robotic manipulators was proposed 
(Chan & Dubey, 1995) as follows: 



VI 1 V<7,,max ^/,min) 

f-1 Vj/.max J i ^current > \^i,current j/.min/ 



//(g) = y! ^'■ max q, - mm) (17) 
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where "q" is the angle of joint "i". This criterion function becomes "1" when the current 
joint angle is in the middle of its range, and it becomes "infinity" when the joint reaches 
either of its limits. Using this optimization function in (14) can be accomplished through the 
weight matrix used for optimization. Rather than choosing arbitrary weight values for each 
individual joint based on the user preference only, an additional value can be added to 
represent the optimization criterion function as follows: 



W ■- 



w \ u + 


SH(q) 









dq { 







H» 2 + 


8H(q) 






dq 2 










... w, 


u + 


dH(q) 


dq, 



(18) 



where "wj iU " is the user-defined weight preference to joint "i" , and the second term in each 
element is the gradient projection of the criterion function defined as: 



dH(q) = (g,, max -g, mi „) 2 ' (2 ■ g,, twfM ~ g, max -g, im J 
d <l, 4 ■ («W " ?/.„„,,„, ) 2 ' (?,-.„,.,„, " ?,-, mi „ ) 2 



(19) 



When any particular joint is in the middle of the joint range, (19) becomes zero for that joint, 
and the only weight left is the user defined weight. On the other hand, when any particular 
joint is at its limit, (19) becomes "infinity", which means that the joint will carry an infinite 
weight that makes it impossible to move any further. When the user prefers to move the 
robotic arm with minimal wheelchair motion, heavy weight can be assigned to the two 
wheelchair state variables. When any of the robotic arm joints gets close to its limit and its 
weight approaches infinity, the wheelchair's weight will be much less than that of the joint, 
and hence it will be more free to move than the joint that is close to its limit. 
It is important to note two different deficiencies that may lead to unintended operation or 
"joint lock" when using this method. The first deficiency is that the joint is penalized with 
higher weight whether it is approaching its limit or getting away from it. This may cause the 
robotic arm to use the null space inefficiently by preferring to move a joint with heavy 
weight going towards its limit rather than moving a joint with heavier weight that is moving 
away from its limit. This problem was eliminated using the first two conditions in (20) on 
the criterion function to identify the direction of approach (towards or away from) the joint 
limit. The second deficiency is that the precise joint limit that takes the weight to "infinity" 
may never be reached, instead, the numerical solution with its relatively coarse step size 
may jump from a joint value close to the joint limit before it is reached to a joint value close 
to the joint limit after it is reached. This will result in a heavy weight that will slowly get 
lower as the joint gets away from the set limit towards its actual limit. If the previous two 
conditions were applied alone, the result could be a dangerous motion that gives the weight 
as "Wi, u " only since the joint is getting away from its limit from inside that limit. This can 
either break the joint or lock it when it reaches its actual physical limit with the hard stop. 
To overcome this deficiency, the last two conditions in (20) were imposed on the criterion 
function to identify whether the joint is within its limit, or the limit is exceeded. 
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Imposing the above four conditions (see figure 5), on the weight matrix to perform on the 
optimization criterion gave the control mechanism much better results in terms of joint limit 
avoidance and user-preferred motion of each individual variable in the joint space The 
following condition statement summarizes the conditions imposed in the control code: 



w.=w. +< 



dH(q) 



% 



when q min < q t < q n 



when q min < q t < q n 



oo when q min > q t > q n 



when q min > q. > q n 



& A^>0 
dq t 

& A«,0 



dq t 
& A^^>0 



8q t 



(20) 



Two different trajectory generation functions were implemented in the control system when 
autonomous operation option was chosen: linear trajectory and polynomial trajectory with 
parabolic blending (Craig, 2003). The end point can either be given by the user, or can be 
obtained from an on-board laser pointer, as shown in Figure 6. Three different control 
reference frames were programmed so that the user can choose the most suited based on the 
task at hand: The ground frame for autonomous operation with pre-set tasks; the 
wheelchair's frame for wheelchair motion in the most part; and the end-effector's frame for 
teleoperation using the end-effector. The option of controlling the arm alone, the wheelchair 
alone, or the combined wheelchair and arm together was also programmed for the user's 



convenience. 
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Fig. 5. Four joint limit boundary conditions. 
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Fig. 6. Polynomial trajectory from the gripper to the target. 



4. The 9-DoF WMRA System 

4.1 Hardware Design of the Arm 

An entirely new WMRA was developed, designed and built. The goal was to produce an 
arm that has better manipulability, greater pay load, and easier control than current designs. 
As found in previous research (Edwards et al., 2006), side mounting is preferable overall 
because it provides the best balance between manipulability and unobtrusiveness. This 
mounting location allows the arm to be stowed by folding it back and then wrapping the 
forearm behind the seat. This helps avoid the stigma that these devices can bring. It virtually 
disappears when not in use, especially when the arm is painted to match the chair. 
However, care must be taken to prevent widening of the power chair. Our arm only 
increases chair width by 7.5cm. 

This manipulator is intended for use in Activities of Daily Living (ADL), and for job tasks of 
a typical office environment. As such, it is important that the arm be strong enough to move 
objects that are common in these environments. Approximately 4 kg mass is set as the upper 
limit for a typical around-the-house object that must be manipulated. This was set as the 
baseline payload for the arm at full horizontal reach at rest. Then, an extra margin of 2 kg 
was added to allow for a choice of end-effector capable of this load. Reconfigurable arm 
lengths allow greater leverage on the engineering input, as a single basic design may be 
adapted to numerous applications. This is only practical with electric drive and actuator 
placement directly at each joint. 
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Fig. 7. Complete SolidWorks model of the arm. 
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Fig. 8. Kinematic diagram with link frame assignments. 

In the power wheelchair industry, a 24-volt lead-acid battery pack is standard, and is the 
natural choice for the power supply of a WMRA with minimum power consumption. To 
have a widespread adoption of these devices, reasonable cost is important. The target was to 
be in the mid-range of commercially available systems in terms of cost. Extra degrees of 
freedom help improve manipulability. This is evidenced by the considerable increase going 
from Raptor's 4 DoF to the 6 DoF of MANUS. Our new design incorporates 7 joints, 
allowing full pose control even in difficult regions of the workspace, such as reaching 
around the wheelchair, or up to a high shelf. 

The arm is a 7-DoF design, using 7 revolute joints. It is anthropomorphic, with joints 1, 2 and 
3 acting as a shoulder, joint 4 as an elbow, and joints 5, 6 and 7 as a wrist as shown in Figure 
7. The 3 DoF shoulder allows the elbow to be positioned anywhere along a spherical surface, 
whereas with the Raptor arm, elbow movement is limited to a circle. Throughout the arm, 
adjacent joint axes are oriented at 90 degrees as shown in Figure 8. This helps to meet two 
goals: mechanical design simplicity and kinematic simplicity with low computational cost. 
All adjacent joint axes intersect, also simplifying the kinematics. The basic arrangement for 
each joint is a high-reduction gearhead, a motor with encoder and spur-gear reduction, and 
a bracket that holds these two parts and attaches to the two neighbouring links. 



4.2 Hardware Design of the Controller 

As shown in Fig 9, PIC-SERVO SC controllers (CI through C7) that support the DC servo 
actuators (Jl through J7) were chosen. At 5cm x 7.5cm, this unit has a microprocessor that 
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drives the built-in amplifier with a PWM signal, handles PID position / velocity control, 
communicates with RS-485, and can be daisy-chained with up to 32 units. It also reads 
encoders, limit switches, an 8 bit analogue input, and supports coordinated motion control. 
Data for the entire arm is interfaced to the main computer using a single serial link. The PIC- 
Servo SC controllers use RS-485, and a hardware converter interfaces this with the RS-232 or 
a USB port on the host PC. A timer has been utilized to cut the arm's power off after a preset 
time to minimize power consumption while not in use. An emergency stop button is placed 
to cut the power off the motors and leave the logic power on so that the system can be 
diagnosed without rebooting. 

The current host PC is an IBM laptop, running Windows XP. However, the communications 
protocol is simple and open, and could be adapted to virtually any hardware/ software 
platform with an RS-232 or USB port. Currently, the tested user interfaces are the keyboard 
and a SpaceBall controller. 

4.3 Wheelchair Modification 

Figure 10 shows the WMRA system installed on the modified wheelchair "Action Ranger X 
Storm Series". The wheelchair has been modified by adding an incremental encoder on each 
one of the wheels. The controller module of the wheelchair has also been modified using 
TTL compatible signal conditioner and a DA converter so that the signal going to the wheels 
can be controlled using the same PIC-Servo SC controllers used in the arm. The only 
difference is that the output from this control board used for the wheelchair is the PWM 
signal rather than the amplified analogue signal. Two more PIC-Servo SC controllers were 
added to the control system shown in Figure 9 to control the wheelchair. 
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Fig. 9. Control system circuitry of the arm. 
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Fig. 10. WMRA SolidWorks models and the built device. 



4.4 Hardware Design of an Ergonomic Gripper 

A new robotic gripper was designed and constructed (Alqasemi et al., 2007) for Activities of 
Daily Living (ADL) to be used with the new WMRA. As shown if Figure 11, two aspects of 
the new gripper made it unique; one is the design of the paddles, and the other is the design 
of the actuation mechanism that produces parallel motion for effective gripping. The 
paddles of the gripper were designed to grasp a wide variety of objects with different 
shapes and sizes that are used in every day life as shown in Figure 12. The driving 
mechanism was designed to be simple, light, effective, safe, self content, and independent of 
the robotic arm attached to it. 




Fig. 12. Using the new gripper in typical ADL tasks. 
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with Encoder 



Fig. 11. The designed ergonomic gripper. 



4.5 Simulation of the WMRA System in Virtual Reality 

The control methods that combined the manipulation and mobility of the newly developed 
WMRA were tested in simulation before applying them to the actual WMRA system. This 
step is very important for debugging and inspecting the methods before applying them into 
the actual arm so that no harm to the physical system is done in case of unexpected errors. 
In the control software, several options were made available to include the modularity, re- 
configurability and flexibility requirements for this WMRA system. 

The control system was implemented in simulation using Matlab 2008 with Virtual Reality 
toolbox installed on a PC running Windows XP. Modules of small programs were generated 
for different operations and different user interfaces, and a main program that uses the 
modules was developed to simulate the WMRA system using different control parameters 
and user interfaces, including a Graphical User Interface (GUI). Figure 13 shows a sample of 
the Virtual Reality simulation. 




Fig. 13. Sample of the virtual reality simulation at the initial position. 
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5. The Integration of the Brain-Computer Interface 

In the program structure, flexibility was one of the objectives in the design of user interfaces 
so that a wider range of these interfaces can be used based on the user's abilities and 
preference. A six-axis, twelve-way SpaceBall that is capable of moving in the six Cartesian 
coordinates was implemented. A computer keyboard and a mouse was also used with the 
on-screen graphical user interface. A touch screen with a choice of robotic actions was 
programmed as one of the integrated interfaces to the system. Figure 14 shows some of the 
user interface device options used in the system. 

Another user interface used in this work is the Brain-Computer Interface (BO). Over the 
past two decades, a variety of studies have evaluated the possibility that brain signals 
recorded from the scalp or from within the brain could provide new augmentative 
technology that does not require muscle control (Schalk et al., 2004). 




Fig. 14. User interfaces, left to right: SpaceBall, touch screen and GUI. 



These BCI systems measure specific features of brain activity and translate them into device 
control signals as shown in Figure 15. This brain activity can be elicited using a framework 
called the "oddball paradigm". Studies have shown that when subjects are assigned a task of 
categorizing an item into 2 possible categories, and one of the two categories occurs 
infrequently, those events in that rare category will elicit an event-related brain potential 
(ERP) with latency of about 300 ms, labeled the P300 (Farwell & Donchin, 1988). The P300 is 
a neural evoked potential component of the EEG, or electroencephalogram (Sutton et al., 
1965). It is supposed to follow unexpected sensory stimuli that provide useful information to 
the subjects according to his/her task. For easier, non-invasive use of this neuro-imaging 
technology, the user wears a head cap fitted with several electrodes to measure the P300 
EEG signals from the activities of the brain. 

In this work, the subjects viewed a 5x3 matrix whose rows and columns intensify randomly. 
Each of the 15 symbols in the matrix corresponds to a specific direction or task command as 
shown in Figures 15&16. The subject focuses attention on the desired cell carrying his/her 
desired task or direction of motion. Every time the user counts one more view of the symbol, 
the P300 EEG signal is recorded, and the corresponding row or column that was shown at 
that moment was also recorded. In about 15 seconds, the BCI gives the selected row and 
column of the matrix on the screen as only the row and column containing target cell elicit a 
P300. The system then translates the chosen character to a corresponding command, which 
translates into a Cartesian velocity in the proper direction and executes the algorithm to 
move the arm. As the detection of P300 requires signal averaging, number of trials is 
required by the system to correctly determine user's intention. The speed of the system thus 
depends on the number of sequences of flashes required to achieve a given level of accuracy. 
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Fig. 15. Basic design and operation of the BCI system. 




Fig. 16. WMRA user with BCI electrode cap. 
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Six able-bodied young adults were able to control the WMRA system movements using the 
BCI. Participants sat on the wheelchair, wearing a 16-channel electrode cap (Electro Cap 
International, Inc.) and attended to different symbols on the screen. Every 50 ms a row or a 
column intensified for 75 ms. For a 5x3 matrix, each sequence of flashes contained 8 
intensifications (5 columns and 3 rows) and lasted for 1000 ms. The BCI was also trained to 
be optimized for each particular human subject, and it showed high accuracy of the selected 
choice (ranging from 92% to 100 %). These gains were recorded and used for the actual test. 
We tested the accuracy of character selection as a function of number of sequences of flashes. 
During the testing phase, a successful control with high accuracy of the motion response 
was apparent. 

6. Experimental Results 

The control method used in this work to combine mobility and manipulation in redundant 
mobile robots was tested using the developed Matlab program that can simulate the WMRA 
motion and control the physical WMRA system with various user interfaces. The WMRA 
was commanded to go in an autonomous mode from its default initial position shown in 
Figure 17 to a defined point in space for the end effector. Several methods were tested in this 
simulation. In this paper, we will limit our findings to the Weighted Least Norm solution 
control, and we will discuss the system response using different control methods in an 
extreme case where the arm was commanded to go to an out-of-reach position. 

6.1 Simulation Results Using Different Weights 

Three different values were tried for the diagonal elements of the weight matrix "W/' to 
implement the control system and to verify its effectiveness in damping the wheelchair 
motion or the arm motion. Figures 18 through 20 show the final poses of the WMRA system 
after the end-effector reached the desired destination for the five cases studied. 




Fig. 17. Initial WMRA pose. 



Fig. 18. Destination pose - Wd = [1, 1, 1, 1, 1, 

1,1,1,1]. 
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Fig. 19. Destination pose - Wd = [10, 10, 10, 
10,10,10,10,1,1]. 



Fig. 20. Destination pose - Wd = [1, 1, 1, 1, 1, 

1,1,100,100]. 



The weight matrix of the first case carried in its diagonal elements "1" for each of the arm's 
seven joints, and "1" for wheelchair's position and orientation variables, which means that 
the wheelchair's two variables and the arm's joints carry the same tendency for motion, as 
shown in Figure 18. In this case, the weight matrix is useless since it is identity. The weight 
matrix of the second case carried in its diagonal elements "10" for each of the arm's seven 
joints, and "1" for wheelchair's position and orientation variables, which means that the 
wheelchair's two variables are 10 times more likely to move than the arm's joints, and that is 
apparent in the results shown in Figure 19. In the third case, "Wd" carried weights of "1" for 
the arm's seven joints, and a weight of "100" for the wheelchair's position and orientation. 
This means that the arm's joints are 100 times more likely to move than the wheelchair's two 
variables, and Figure 20 shows how the wheelchair's motion was minimal. 
The simulation program was designed to give different useful values and plots throughout 
the simulation process for observation and diagnosis of any potential problems that might 
occur during the task execution. In the first case, when all 9 variables carried the same 
weights, an apparent motion in the arm and the wheelchair alike occurred. In the second 
case, when the arm carried a heavy weight in the weight matrix, it was clear that the seven 
arm joints had minimal motion that was necessary for the destination to be reached. That 
end-effector destination was impossible to reach by using the two wheelchair's variables 
only. The third case shows an easy arm motion and very minimal wheelchair motion that 
was necessary to avoid singularity. 

An important property of this optimization method was apparent during simulation, which 
was the minimization of singularity. As the arm was moving to the destination and both 
wheels were moving backwards, the wheels reversed their motion in the middle of the 
simulation period when the arm started approaching singularity. Figures 21 through 23 
show the manipulability index of both arm only and the combined WMRA system. It is 
important to note here that these values were multiplied by ( 10~ 9 ) to get the normalized 
manipulability measure. It is clear that the manipulability is much higher for the WMRA 
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system than that of the arm only due to the fact that the WMRA system carries two more 
degrees of freedom. 
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Fig. 21. Manipulability index - Wd = [1, 1, 1, 1, 1, 1, 1, 1, 1]. 



5 

4.5 

4 

£ 35 

Z5 
Cfl 

CD -- 1 

^2.5 

.a 

1 2 

'"P 

2 1.5 

1 

0.5 





x10 



I 8 Manipulability Measure vs Time 






■ W A 

■w,„ 



4 6 

time, (seel 



10 12 



Fig. 22. Manipulability index - Wd = [10, 10, 10, 10, 10, 10, 10, 1, 1]. 
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Fig. 23. Manipulability index - Wd = [1, 1, 1, 1, 1, 1, 1, 100, 100]. 

In all three cases, the manipulability measure was maximized based on the weight matrix. 
Figure 21 shows an improvement trend of the WMRA's manipulability index over the arm's 
manipulability index towards the end of simulation. Figure 22 shows the manipulability of 
the arm as nearly constant compared to that in Figure 23 because of the minimal motion of 
the arm. Figure 23 shows how the wheelchair started moving rapidly later in the simulation 
(see figure 20) as the arm approached singularity, even though the weight of the wheelchair 
motion was heavy. This helped in improving the WMRA system's manipulability. 



6.2 Simulation Results in an Extreme Case 

To test the difference in the system response when using different methods, an extreme case 
was tested, where the WMRA system is commanded to reach a point that is physically 
unreachable. The end-effector was commanded to move horizontally and vertically 
upwards to a height of 1.3 meters from the ground, which is physically unreachable, and the 
WMRA system will reach singularity. The response of the system can avoid that singularity 
depending on the method used. Singularity, joint limits and preferred joint-space weights 
were the three factors we focused on in this part of the simulation. Eight control cases 
simulated were as follows: 

(a) Case I: Pseudo inverse solution (PI): In this case, the system was unstable, the joints 
went out of bounds, and the user had no weight assignment choice. 

(b) Case II: Pseudo inverse solution with the gradient projection term for joint limit 
avoidance (PI-JL): In this case, the system was unstable, the joints stayed in bounds, and 
the user had no weight assignment choice. 

(c) Case III: Weighted Pseudo inverse solution (WPI): In this case, the system was unstable, 
the joints went out of bounds, and the user had weight assignment choices. 
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(d) Case IV: Weighted Pseudo inverse solution with joint limit avoidance (WPI-JL): In this 
case, the system was unstable, the joints stayed in bounds, and the user had weight 
assignment choices. 

(e) Case V: S-R inverse solution (SRI): In this case, the system was stable, the joints went 
out of bounds, and the user had no weight assignment choice. 

(f) Case VI: S-R inverse solution with the gradient projection term for joint limit avoidance 
(SRI-JL): In this case, the system was unstable, the joints stayed in bounds, and the user 
had no weight assignment choice. 

(g) Case VII: Weighted S-R inverse solution (WSRI): In this case, the system was stable, the 
joints went out of bounds, and the user had weight assignment choices. 

(h) Case VIII: Weighted S-R inverse solution with joint limit avoidance (WSRI-JL): In this 
case, the system was stable, the joints stayed in bounds, and the user had weight 
assignment choices. 
In the first case, Pseudo inverse was used in the inverse Kinematics without integrating the 
weight matrix or the gradient projection term for joint limit avoidance. Figure 24 shows how 
this conventional method led to the singularity of both the arm and the WMRA system. The 
user's preference of weight was not addressed, and the joint limits were discarded. In the 
last case, the developed method that uses weighted S-R inverse and integrates the gradient 
projection term for joint limit avoidance was used in the inverse kinematics. Figure 25 shows 
the best performance of all tested methods since it fulfilled all the important control 
requirements. This last method avoided singularities while keeping the joint limits within 
bounds and satisfying the user-specified weights as much as possible. The desired trajectory 
was followed until the arm reached its maximum reach perpendicular to the ground. Then it 
started pointing towards the current desired trajectory point, which minimizes the position 
errors. Note that the arm reaches the minimum allowed manipulability index, but when 
combined with the wheelchair, that index stays farther from singularity. 
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Fig. 24. Manipulability index - using only Pseudo inverse in an extreme case. 
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It is important to mention that changing the weights of each of the state variables gives 
motion priority to these variables, but may lead to singularity if heavy weights are given to 
certain variables when they are necessary for particular motions. For example, when the 
seven joints of the arm were given a weight of "1000" and the task required rapid motion of 
the arm, singularity occurred since the joints were nearly stationary. Changing these 
weights dynamically in the control loop depending on the task in hand leads to a better 
performance. This subject will be explored and published in a later publication. 
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Fig. 25. Manipulability index - using weighted S-R inverse with the gradient projection term 
for joint limit avoidance in an extreme case. 



6.3 Clinical Testing on Human Subjects 

In the teleoperation mode of the testing, several user interfaces were tested. Figure 29 shows 
the WMRA system with the Barrette hand installed and a video camera used by a person 
affected by Guillain-Barre Syndrome. In her case, she was able to use both the computer 
interface and the touch-screen interface. Other user interfaces were tested, but in this paper, 
we will discuss the BCI user interface results. When asked, participants informed the tester 
that they preferred the 4 and 6 sequences of flashes over the longer sequences. The common 
explanation was that it was easier to stay focused for shorter periods of time. Figure 30 
shows accuracy data obtained when participants spelled 50 characters of each set of 
sequences (12, 10, 8, 6, 4, and 2). As the number of sequences of flashes decrease, the speed 
of the BCI system increases as the maximum number of characters read per unit of time 
increases. This compromise affects the accuracy of the selected characters. Figure 31 shows 
the mean percentages correct for each of the sequences. The percentages are presented as 
number of maximum characters per minute. 

The results call for the evaluation of the speed accuracy trade-off in an online mode rather 
than in an offline analysis to account for the users' ability to attend to a character over time. 
Few potential problems were noticed as follows: Every full scan of a single user input takes 
about 15 second, and that might cause a delay in the response of the WMRA system to 
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change direction on time as the human user wishes. This 15-second delay may cause 
problems in case the operator needs to stop the WMRA system for a dangerous situation 
such as approaching stairs, or if the user made the wrong selection and needed to return 
back to his original choice. 




Fig. 29. A person with Guillain-Barre Syndrome driving the WMRA system. 
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Fig. 30. Accuracy data (% correct) for 6 human subjects. 
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Fig. 31. Accuracy data (% correct) for each of the flash sequences. 



It is also noted that after an extended period of time in using the BCI system, fatigue starts 
to appear on the user due to his concentration on the screen when counting the appearances 
of his chosen symbol. This tiredness on the user's side can be a potential problem. 
Furthermore, when the user needs to constantly look at the screen and concentrate on the 
chosen symbol, This will distract him from looking at where the WMRA is going, and that 
poses some danger on the user. Despite the above noted problems, a successful interface 
with a good potential for a novel application was developed. Further refinement of the BCI 
interface is needed to minimize potential risks. 



7. Conclusions and Recommendations 

A wheelchair-mounted robotic arm (WMRA) was designed and built to meet the needs of 
mobility-impaired persons, and to exceed the capabilities of current devices of this type. 
Combining the wheelchair control and the arm control through the augmentation of the 
Jacobian to include representations of both resulted in a control system that effectively and 
simultaneously controls both devices at once. The control system was designed for 
coordinated Cartesian control with singularity robustness and task-optimized combined 
mobility and manipulation. Weighted Least Norm solution was implemented to prioritize 
the motion between different arm joints and the wheelchair. 

Modularity in both the hardware and software levels allowed multiple input devices to be 
used to control the system, including the Brain-Computer Interface (BCI). The ability to 
communicate a chosen character from the BCI to the controller of the WMRA was presented, 
and the user was able to control the motion of WMRA system by focusing attention on a 
specific character on the screen. Further testing of different types of displays (e.g. 
commands, picture of objects, and a menu display with objects, tasks and locations) is 
planned to facilitate communication, mobility and manipulation for people with severe 
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disabilities. Testing of the control system was conducted in Virtual Reality environment as 

well as using the actual hardware developed earlier. The results were presented and 

discussed. 

The authors would like to thank and acknowledge Dr. Emanuel Donchin, Dr. Yael Arbel, 

Dr. Kathryn De Laurentis, and Dr. Eduardo Veras for their efforts in testing the WMRA with 
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1. Introduction 

Industrial robots are reprogrammable, multifunctional manipulators designed to move 
parts, materials, and devices through computer controlled motions. A robot application 
program is a set of instructions that cause the robot system to move the robot's end-of-arm- 
tooling (or end-effector) to robot points for performing the desired robot tasks. Creating 
accurate robot points for an industrial robot application is an important programming task. 
It requires a robot programmer to have the knowledge of the robot's reference frames, 
positions, software operations, and the actual programming language. In the conventional 
"lead-through" method, the robot programmer uses the robot teach pendant to position the 
robot joints and end-effector via the actual workpiece and record the satisfied robot pose as 
a robot point. Although the programmer's visual observations can make the taught robot 
points accurate, the required teaching task has to be conducted with the real robot online 
and the taught points can be inaccurate if the positions of the robot's end-effector and 
workpiece are slightly changed in the robot operations. Other approaches have been utilized 
to reduce or eliminate these limitations associated with the online robot programming. This 
includes generating or recovering robot points through user-defined robot frames, external 
measuring systems, and robot simulation software (Cheng, 2003; Connolly, 2006; 
Pulkkinenl et al., 2008; Zhang et al, 2006). 

Position variations of the robot's end-effector and workpiece in the robot operations are 
usually the reason for inaccuracy of the robot points in a robot application program. To 
avoid re-teaching all the robot points, the robot programmer needs to identify these position 
variations and modify the robot points accordingly. The commonly applied techniques 
include setting up the robot frames and measuring their positional offsets through the robot 
system, an external robot calibration system (Cheng, 2007), or an integrated robot vision 
system (Cheng, 2009; Connolly, 2007). However, the applications of these measuring and 
programming techniques require the robot programmer to conduct the integrated design 
tasks that involve setting up the functions and collecting the measurements in the 
measuring systems. Misunderstanding these concepts or overlooking these steps in the 
design technique will cause the task of modifying the robot points to be ineffective. 
Robot production downtime is another concern with online robot programming. Today's 
robot simulation software provides the robot programmer with the functions of creating 
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virtual robot points and programming virtual robot motions in an interactive and virtual 3D 
design environment (Cheng, 2003; Connolly, 2006). By the time a robot simulation design is 
completed, the simulation robot program is able to move the virtual robot and end-effector 
to all desired virtual robot points for performing the specified operations to the virtual 
workpiece without collisions in the simulated workcell. However, because of the inevitable 
dimensional differences of the components between the real robot workcell and the 
simulated robot workcell, the virtual robot points created in the simulated workcell must be 
adjusted relative to the actual position of the components in the real robot workcell before 
they can be downloaded to the real robot system. This task involves the techniques of 
calibrating the position coordinates of the simulation Device models with respect to the 
user-defined real robot points. 

In this chapter, advanced techniques used in creating industrial robot points are discussed 
with the applications of the FANUC robot system, Delmia IGRIP robot simulation software, 
and Dynalog DynaCal robot calibration system. In Section 2, the operation and 
programming of an industrial robot system are described. This includes the concepts of 
robot's frames, positions, kinematics, motion segments, and motion instructions. The 
procedures for teaching robot frames and robot points online with the real robot system are 
introduced. Programming techniques for maintaining the accuracy of the exiting robot 
points are also discussed. Section 3 introduces the setup and integration of a two 
dimensional (2D) vision system for performing vision-guided robot operations. This 
includes establishing integrated measuring functions in both robot and vision systems and 
modifying existing robot points through vision measurements for vision-identified 
workpieces. Section 4 discusses the robot simulation and offline programming techniques. 
This includes the concepts and procedures related to creating virtual robot points and 
enhancing their accuracy for a real robot system. Section 5 explores the techniques for 
transferring industrial robot points between two identical robot systems and the methods 
for enhancing the accuracy of the transferred robot points through robot system calibration. 
A summary is then presented in Section 6. 

2. Creating Robot Points Online with Robot 

The static positions of an industrial robot are represented by Cartesian reference frames and 
frame transformations. Among them, the robot base frame R(x, y, z) is a fixed one and the 
robot's default tool-center-point frame Def_TCP (n, o, a), located at the robot's wrist 
faceplate, is a moving one. The position of frame Def_TCP relative to frame R is defined as 
the robot point pr n l R and is mathematically determined by the 4x4 homogeneous 

transformation matrix in Eq. (1) 



(1) 



where the coordinates of vector p = (p x , p y , p z ) represent the location of frame Def_TCP and 
the coordinates of three unit directional vectors n, o, and a represent the orientation of frame 
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Def TCP. The inverse of R Tr 



R )-l 

Def _ TCP ' 



r D ef_TCP or p[ n ]g ef TCP denoted as (RToeLTCp)" 1 or ( P [ n ] 

represents the position of frame R to frame Def_TCP, which is equal to frame transformation 
Def_TCPX R . Generally, the definition of a frame transformation matrix or its inverse described 
above can be applied for measuring the relative position between any two frames in the 
robot system (Niku, 2001). The orientation coordinates of frame DefJTCP in Eq. (1) can be 
determined by Eq. (2) 
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where transformations Rot(x, 6 X ), Rot(y, By), and Rot(z, 8 Z ) are pure rotations of frame 
Def_TCP about the x-, y-, and z-axes of frame R with the angles of 8 X (yaw), 8 y (pitch), and 8 Z 
(roll), respectively. Thus, a robot point pfnlS T rp can a ^ so ^ e re P reser >ted by Cartesian 
coordinates in Eq. (3) 



p [ n ]i)ef_TCP =(x,y,z,w,p,r)- 



(3) 



It is obvious that the robot's joint movements are to change the position of frame Def_TCP. 
For an n-joint robot, the geometric motion relationship between the Cartesian coordinates of 
a robot point p[nl R m frame R (i.e. the robot world space) and the proper 

displacements of its joint variables q = (qi, qz, ..q n ) in robot joint frames (i.e. the robot joint 
space) is mathematically modeled as the robot's kinematics equations in Eq. (4) 
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(4) 



where fij(q, r) (for i = 1, 2, 3 and j = 1, 2, 3, 4) is a function of joint variables q and joint 

parametersr. 

Specifically, the robot forward kinematics equations will enable the robot system to determine 

where a pr n l R will be if the displacements of all joint variables q=(qi, qz, .qn) are known. 

The robot inverse kinematics equations will enable the robot system to calculate what 
displacement of each joint variable qk (for k = 1 ,..., n) must be if a p[ n ] R is specified. If the 

inverse kinematics solutions for a given pr n l R are infinite, the robot system defines the point 

as a robot "singularity" and cannot move frame DefJTCP to it. 
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In robot programming, the robot programmer creates a robot point pr n l R by first declaring 

it in a robot program and then defining its coordinates in the robot system. The conventional 
method is through recording a particular robot pose with the robot teach pendent (Rehg, 2003). 
Under the teaching mode, the robot programmer jogs the robot's joints for poisoning the robot's 
end-effector relative to the workpiece. As joint k moves, the serial pulse coder of the joint 
measures the joint displacement qk relative to the "zero" position of the joint frame. The robot 
system substitutes all measured values of q = (qi, q2, ..q n ) into the robot forward kinematics 
equations to determine the corresponding Cartesian coordinates of frame Def_TCP in Eq. (1) and 
Eq. (3). After the robot programmer records a pf n l R with the teach pendant, its Cartesian 

coordinates and the corresponding joint values are saved in the robot system. The robot 
programmer may use the "Representation" softkey on the teach pendant to automatically 
convert and display the joint values and Cartesian coordinates of a taught robot point 



P[n] 



It is important to notice that Cartesian coordinates in Eq. (3) is the standard 



representation of a pr n l R m the industrial robot system, and its joint representation always 

uniquely defines the position of frame Def_TCP (i.e. the robot pose) in frame R. 
In robot programming, the robot programmer defines a motion segment of frame Def_TCP by 
using two taught robot points in a robot motion instruction. During the execution of a motion 
instruction, the robot system utilizes the trajectory planning method called "linear segment with 
parabolic blends" to control the joint motion and implement the actual trajectory of frame 
Def_TCP through one of the two user-specified motion types. The "joint" motion type allows the 
robot system to start and end the motion of all robot joints at the same time resulting in an 
unpredictable, but repeatable trajectory for frame Def_TCP. The "Cartesian" motion type allows 
the robot system to move frame Def_TCP along a user-specified Cartesian path such as a straight 
line or a circular arc in frame R during the motion segment, which is implemented in three steps. 
First, the robot system interpolates a number of intermediate points along the specified Cartesian 
path in the motion segment. Then, the proper joint values for each interpolated robot point are 
calculated by the robot inverse kinematics equations. Finally, the "joint" motion type is applied 
to move the robot joints between two consecutive interpolated robot points. 

Different robot languages provide the robot systems with motion instructions in different format. 
The motion instruction of FANUC Teach Pendant Programming (TPP) language (Fanuc, 2007) 
allows the robot programmer to define a motion segment in one statement that includes the 
robot point P[n], motion type, speed, motion termination type, and associated motion options. 
Table 1 shows two motion instructions used in a FANUC TP program. 



FANUC TPP Instruction 


Description 


1. J P[l] 50% FINE 


Moves the TCP frame to robot point P[l] 
with "Joint" motion type (J) and at 50% of 
the default joint maximum speed, and stops 
exactly at P[l] with a "Fine" motion 
termination. 


2. L P[2] 100 mm/ sec FINE 


Utilizes "Linear" motion type (L) to move 
TCP frame along a straight line from P[l] to 
P[2] with a TCP speed of 100 mm/ sec and a 
"Fine" motion termination type. 



Table 1. Motion instructions of FANUC TPP language 
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2.1 Design of Robot User Tool Frame 

In the industrial robot system, the robot programmer can define a robot user tool frame 
UT[k](x, y, z) relative to frame Def_TCP for representing the actual tool-tip point of the 
robot's end-effector. Usually, the UT[k] origin represents the tool-tip point and the z-axis 
represents the tool axis. A UT[k] plays an important role in robot programming as it not 
only defines the actual tool-tip point but also addresses its variations. Thus, every end- 
effector used in a robot application must be defined as a UT[k] and saved in robot system 
variable UTOOL[k]. Practically, the robot programmer may directly define and select a 
UT[k] within a robot program or from the robot teach pendant. Table 2 shows the UT[k] 
frame selection instructions of FANUC TPP language. When the coordinates of a UT[k] is 
set to zero, it represents frame Def_TCP. The robot system uses the current active UT[k] to 
record a robot point Pfnlfn-rn as shown in Eq. (5) and cannot move the robot to any robot 

point pfml R ^at is taught with a UT[g] different from UT[k] (i.e. g 5 s k). 

P [ n ]uT[k]= RT UT[k] ( 5 ) 

It is obvious that a robot point P[n]£ T rp m ^T 0-) or ^1' P) can ^° e taught with different 
UT[k], thus, represented in different Cartesian coordinates in the robot system as shown in 
Eq. (6) 

P[ n ]uT[k] =P[ n ]Def_TCP X ~ T UT [ k ] ■ (°) 



FANUC TPP Instruction 


Description 


1. UTOOL_NUM=l 


Set UT[1] frame to be the current active 
UT. 



Table 2. UT[k] frame selection instructions of FANUC TPP language 

To define a UT[k] for an actual tool-tip point P-r-Ref whose coordinates (x, y, z, w, p, r) in 
frame D e f tcp is unknown, the robot programmer must follow the UT Frame Setup 
procedure provided by the robot system and teach six robot points PfnlS 

(for n = 1, 2, ... 6) with respect to P-r-Ref and a reference point Ps-Ref on a tool reachable 
surface. The "three-point" method as shown in Eq. (7) and Eq. (8) utilizes the first three 
taught robot points in the UT Frame Setup procedure to determine the UT[k] origin. 
Suppose that the coordinates of vector Def_TCpp= [p^ p o ^ pjT represent point P-r-Ref in frame 
Def_TCP. Then, it can be determined in Eq. (7) 

°ef-TCP p = (Ti) -l x R p/ (7) 

where the coordinates of vector R p= [p x , p y , p z ] T represents point P-r-Ref in frame R and Ti 
represents the first taught robot point pfl] R when point P-r-Ref touches point Ps-Ref- The 

coordinates of vector R p= [p x , p y , p z ] T also represents point Ps-Ref in frame R and can be 
solved by the three linear equations in Eq. (8) 
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(I-T 2 T 3 - 1 )x R p = 



(8) 



where transformations T2 and T3 represent the other two taught robot points p[2] R and 

PPln Trp m *- ne UT" Frame Setup procedure respectively when point P-r-Ref is at point Ps-Ref- 

To ensure the UT[k] accuracy, these three robot points must be taught with point Pj-Ref 
touching point Ps-Ref from three different approach statuses. Practically, p[21 R (or 

Pf 31n Trp ) can ^ e tau g nt by first rotating frame Def_TCP about its x-axis (or y-axis) for at 

least 90 degrees (or 60 degrees) when the tool is at pm R , and then moving point P-r-Ref 

back to point Ps-Ref- A UT[k] taught with the "three-point" method has the same orientation 
of frame Def TCP. 




Surface 
Reference Point 




TCP 



Fig. 1. The three-point method in teaching a UT[k] 

If the UT[k] orientation needs to be defined differently from frame Def_TCP, the robot 
programmer must use the "six-point" method and teach additional three robot points 
required in UT Frame Setup procedure. These three points define the orient origin point, the 
positive x-direction, and the positive z-direction of the UT[k], respectively. The method of 
using such three non-collinear robot points for determining the orientation of a robot frame 
is to be discussed in section 2.2. 

Due to the tool change or damage in robot operations the actual tool- tip point of a robot's 
end-effector can be varied from its taught UT[k], which causes the inaccuracy of existing 
robot points relative to the workpiece. To aviod re-teaching all robot points, the robot 
programmer needs to teach a new UT[k]' for the changed tool-tip point and shift all existing 
robot points through offset Def - TCP TDef_TCP' as shown in Fig. 2. Assume that transformation 
De£ - TCP TuT[k] represents the position of the original tool-tip point and remains unchanged 
when frame UT[k] changes into new UT[k]' as shown in Eq. (9) 



Def _ TCP r 



■-UT[k]- 



Def_TCP'r 



l UT[k]" 



(9) 



where frame Def_TCP' represents the position of frame Def_TCP after frame UT[k] moves 
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to UT[k]'. In this case, the pre- taught robot point p[ n ] 
corresponding robot point Pfnl?| Trvl . through Eq. (10) 



R r , , can be shifted into the 

UT[k] 



1 UT[k]'' 



L Def _ TCP' 



Def_TCP'rp 



UT[k]' 



(10) 



The industrial robot system usually implements Eq. (9) and Eq. (10) as both a system utility 
function and a program instruction. As a system utility function, the offset Def - TCP TDef_TCP' 
changes the position of frame Def_TCP in the robot system so that the robot programmer is 
able to change the current UT[k] of a taught P[n] into a different UT[k]' while remaining the 
same Cartesian coordinates of P[n] in frame R. As a program instruction, Def - TCP TDef_TCP' 
shifts the pre-taught robot point P[nln Trwl m to the corresponding point Prnl'n Tfvl , without 

changing the position of frame DefJTCP. Table 3 shows the UT[k] offset instruction of 
FANUC TPP language for Eq. (10). 



Def TCP 



Def TCP 



Def _ TCP 




Fig. 2. Shifting a robot point through the offset of frame DefJTCP 



TP Instructions 


Description 


1. Tool_Offset Conditions PR[x], UTOOL[k], 


Offset value Def_TCPT Def _ TCF is 
stored in a user-specified position 
register PR[x]. 


2. J P[n] 100% Fine Tool_Offset 


The "Offset" option in motion 
instruction shifts the existing 
robot point Pfnl R m t° 
corresponding point p^]*^,, • 



Table 3. UT[k] offset instruction of FANUC TPP language 



2.2 Design of Robot User Frame 

In the industrial robot system, the robot programmer is able to establish a robot user frame 
UF[i](x, y, z) relative to frame R and save it in robot system variable UFRAME[i]. A defined 
UF[i] can be selected within a robot program or from the robot teach pendant. The robot 
system uses the current active UF[i] to record robot point PfnlHSIvi as s h° wn m Eq. (11) and 
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cannot move the robot to any robot point P[m] UF ' r j!, that is taught with a UF[j] different 
from UF[i] (i.e. j # i). 

pr nl UF[i] _UF[i] T _ HI) 

1 L n JuT[k] _ i UT[k] \ <" 

It is obvious that a robot point P[nl R m Eq. (1) or Eq. (3) can be taught with different 

UT [k] and UF[i], thus, represented in different Cartesian coordinates in the robot system as 
shown in Eq. (12) 

pr n iUF[i] _/R T r a xPrnl R x Def_TCP T (12) 

1 t Il JuT[k] ~\ J^UFJi]^ xi t Il JDef_TCP x l VT[k] v ; 

However, the joint representation of a Pfnln T rp un iq ue ly defines the robot pose. 

The robot programmer can directly define a UF[i] with a known robot position measured in 
frame R. Table 4 shows the UF[i] setup instructions of FANUC TPP language. 



FANUC TPP Instructions 


Description 


1. UFRAME[i]=PR[x] 


Assign the value of a robot position 
register PR[x] to UF[i] 


2. UFRAME[i]=LPOS 


Assign the current coordinates of frame 
DefJTCP to UF[i] 


3. UFRAME_NUM= i 


Set UF[i] to be active in the robot system 



Table 4. UF[i] setup instructions of FANUC TPP language 

However, to define a UF[i] at a position whose coordinates (x, y, z, w, p, r) in frame R is 
unknown, the robot programmer needs to follow the UF Setup procedure provided by the 
robot system and teach four specially defined points P[n]f m vi ( Ior n = 1, 2, ... 4) where 

UT[k] represents the tool-tip point of a pointer. In this method as shown in Fig. 3, the 
location coordinates (x, y, z) of P[4] (i.e. the system-origin point) defines the actual UF[i] 
origin. The robot system defines the x-, y- and z-axes of frame UF[i] through three mutually 
perpendicular unit vectors a, b, and c as shown in Eq. (13) 

C = axb, (13) 

where the coordinates of vectors a and b are determined by the location coordinates (x, y, z) 
of robot points P[l] (i.e. the positive x-direction point), P[2] (i.e. the positive y-direction 
point), and P[3] (i.e. the system orient-origin point) in R frame as shown in Fig. 3. 
With a taught UF[i], the robot programmer is able to teach a group of robot points relative to 
it and shift the taught points through its offset value. Fig. 4 shows the method for shifting a 
taught robot point P[n] UFI ,' 1 ] with the offset of UF[i]. 
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Fig. 3. The four-point method in teaching a UF[i] 




Fig. 4. Shifting a robot point through the offset of UF[i] 

Assume that transformation UF l i iTuT[k] represents a taught robot point P[n] and remains 
unchanged when P[n] shifts to P[n]' as shown in Eq. (14) 



UF[i] T _UF[i]' T 

1 UT[kI _ 1 UT[k]' 



PMwS^PW^kV 



(14) 



where frame UF[i]' represents the position of frame UF[i] after P[n] becomes P[n]'. Also, 
assume that transformation UF [ i ITuF[i]' represents the position change of UF[i]' relative to 
UF[i], thus, transformation ^PlTuTfk] (or robot point p[nl UF ' i] ) can be converted (or shifted) 

to UFHTuxik]' (or P[n]'$S&, ) as shown in Eq. (15) 



UF[i] T _UF[i] T „UF[i]' T 

-•uTIk] 1- 1 UF[i]' x 1 UT[k] 1 



pr n i'UF[i] _UF[i] T x Plnl UF[iI ■ 

1 L n J UT[k] ,_ J-UFfi] 1 xi L I1 lUT[k] 



(15) 
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Usually, the industrial robot system implements Eq. (14) and Eq. (15) as both a system utility 
function and a program instruction. As a system utility function, offset UF [ i ITuF[i]' changes the 
current UF[i] of a taught robot point P[n] into a different UF[i]' without changing its 
Cartesian coordinates in frame R. As a program instruction, UF I i ]TuF[i]' shifts a taught robot 
point PrnlJSBi hito the corresponding point P[n] lUF l i ' 1 , without changing its original UF[i]. 

Table 5 shows the UF[i] offset instruction of FANUC TPP language for Eq. (15). 



FANUC TPP Instructions 


Description 


3. Offset Conditions PR[x], UFRAME(i), 


Offset value UF l i ITuF[i]' is stored in a user- 
specified position register PR[x]. 


4. J P[n] 100% Fine Offset 


The "Offset" option in motion 
instruction shifts the existing robot point 
pr n i UF ['I into corresponding point 

1 l n i UT[k]' 



Table 5. UF[i] offset instruction of FANUC TPP language 

A robot point Pfn] UF ',!', can a l so be shifted by the offset value stored in a robot position 

register PR[x]. In the industrial robot system, a PR[x] functions to hold the robot position 
data such as a robot point P[n], the current value of frame Def_TCP (LPOS), or the value of a 
user-defined robot frame. Different robot languages provide different instructions for 
manipulating PR[x]. When a PR[x] is taught in a motion instruction, its Cartesian 
coordinates are defined relative to the current active UT[k] and UF[i] in the robot system. 
Unlike a taught robot point Pin] 011 '.',', whose UT[k] and UF[i] cannot be changed in a robot 

program, the UT[k] and UF[i] of a taught PR[x] are always the current active ones in the 
robot program. This feature allows the robot programmer to use the Cartesian coordinates 
of a PR[x] as the offset of the current active UF[i] (i.e. UF PlTuF[i]') in the robot program for 
shifting the robot points as discussed above. 



3. Creating Robot Points through Robot Vision System 

Within the robot workspace the position of an object frame Obj[n] can be measured relative 
to a robot UF[i] through sensing systems such as a machine vision system. Methods for 
integrating vision systems into industrial robot systems have been developed for many 
years (Connolly, 2008; Nguyen, 2000). The utilized technology includes image processing, 
system calibration, and reference frame transformations (Golnabi & Asadpour, 2007; Motta 
et al., 2001). To use the vision measurement in the robot system, the robot programmer must 
establish a vision frame Vis[i](x, y, z) in the vision system and a robot UF[i] ca i(x, y, z) in the 
robot system, and make the two frames exactly coincident. Under this condition, a vision 
measurement represents a robot point as shown in Eq. (16) 



Vis[i] T _UF[i]cal T _ pr -,UF[i]cal 

1 Obj[nI _ F Obj[n] ~ l L n JuT[k] 



(16) 
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3.1 Vision System Setup 

A two-dimensional (2D) robot vision system is able to use the 2D view image taken from a 
single camera to identify a user-specified object and measure its position coordinates (x, y, 
roll) for the robot system. The process of vision camera calibration establishes the vision 
frame Vis[i] (x, y) and the position value (x, y) of a pixel in frame Vis[i]. The robot 
programmer starts the vision calibration by adjusting both the position and focus of the 
camera for a completely view of a special grid sheet as shown in Figure 5a. The final camera 
position for the grid view is the "camera-calibration position" P[n] ca i. During the vision 
calibration, the vision software uses the images of the large circles to define the x- and y- 
axes of frame Vis[i] and the small circles to define the pixel value. The process also 
establishes the camera view plane that is parallel to the grid sheet as shown in Figure 5b. 
The functions of a geometric locator provided by the vision system allow the robot 
programmer to define the user-specified searching window, object pattern, and reference 
frame Obj of the object pattern. After the vision calibration, the vision system is able to 
identify an object that matches the trained object pattern appeared on the camera view 
picture and measure position coordinates (x, y, roll) of the object at position Obj[n] as 
transformation vis P]Tobj[n]. 

3.2 Integration of Vision "Eye" and Robot "Hand" 

To establish a robot user frame UF[i] ca i and make it coincident with frame Vis[i], the robot 
programmer must follow the robot UF Setup procedure and teach four points from the same 
grid sheet this is at the same position in the vision calibration. The four points are the system 
origin point, the X and Y direction points, and the orient origin point of the grid sheet as 
shown in Fig. 5a. 

In a "fixed-camera" vision application, the camera must be mounted at the camera- 
calibration position P[n] ca i that is fixed with respect to the robot R frame. Because frame 
Vis[i] is coincident with frame UF[i] ca i when the camera is at P[n] C ai, the vision measurement 
vis Tobj[np(x, y, roll) to a vision-identified object at position Obj[n] actually represents the 
same coordinates of the object in UF[i] ca i as shown in Eq. (16). With additional values of z, 
pitch, and yaw that can be either specified by the robot programmer or measured by a laser 
sensor in a 3D vision system, vis P]Tobj[n] can be used as a robot point Ernl^'''" 1 m the robot 

program. However, after reaching to vision-defined point pr n '] ur PIc«i ,the robot system cannot 

perform the robot motions with the robot points that are taught via the same vision- 
identified object located at a different position Obj[m] (i.e. m # n). 
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(a) Camera calibration grid sheet 




(b) Vision measurement 



Fig. 5. Vision system setup 



To reuse all pre-taught robot points in the robot program for the vision-identified object at a 
different position, the robot programmer must set up the vision system so that it can 
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determine the position offset of frame UF[i] ca i (i.e. UF [ i l cal TuF[i]'cai) with two vision 
measurements vis Tobj[n] and vis Tobj[m] as shown in Fig. 6 and Eq. (17) 



UF[i]cal^ 



L UF[i]'cal- 



Vis[i] T v /Vis[i] T v-1 

J^Obfflm] x { 1 Obi[n]/ 



and 



(17) 



UF[i]cal T _Vis[i] T _Vis[i]' T _UF[i]'cal T 

J^ObjIn]- J^ObjIn]- ^BJIm] - l Otftm]' 



where frames Vis[i]' and UF[i]' ca i represent the positions of frames Vis[i] and UF[i] ca i after 
object position Obj[n] changes to Obj[m]. Usually, the vision system obtains vis I i ITobj[n] 
during the vision setup and acquires vis P]Tobj[m] when the camera takes the actual view 
picture for the object. 



UT[k] 



Visp] 
UF[i]cal 




Fig. 6. Determining the offset of frame UF[i] ca i through two vision measurements 

In a "mobile-camera" vision application, the camera can be attached to the robot's wrist 
faceplate and moved by the robot on the camera view plane. In this case, frames UF[i] ca i and 
Vis[i] are not coincident each other when camera view position P[m] v ie is not at P[n] ca i. Thus, 
vision measurement Vb WTobj[m] obtained at P[m] V ie cannot be used for determining 
UF[i]caix UF p]- cal in Eq. (17) directly. However, it is noticed that frame Vis[i] is fixed in frame 
Def_TCP and its position coordinates can be determined in Eq. (18) as shown in Fig. 7 



De£_TCP T _C R T 1 

Wisli] ~\ l Det_TCP) 



4 x R T 



UF[i] ca i' 



(18) 



where transformations R TuF[i]cai and R Toef_TCP are uploaded from the robot system when the 
robot-mounted camera is at P[n] ca i during the vision setup. With vision-determined 
Def - TCP Tvis[i], vision measurement Vfa fflTobj[m] can be transformed into UF I i ]Tobj[m] for the robot 
system in Eq. (19) if frame Def_TCP is used as frame UF[i] ca i (i.e. UF[i] ca i = Def_TCP) in the 
robot program as shown in Fig. 7. 



UF[i]calr 



1 Obj[m] 



Def_TCP^ 



1 Obj[m] 



De£_TCP^ 



l Vis[i] 



Vis[i] 



1 Obj[m] 



(19) 



By substituting Eq. (19) into Eq. (17), frame offset UF [ i l cal TuF[i]'cai can be determined in Eq. (20) 
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UF[i]cal - 



l UF[i]'cal~ 



Def_TCPn 



L Vis[i] 



Vis[i] 



■-Objlm] 



/Vis[i] T n-1 

H J^Obirn]^ 



and 



(20) 



UF[i]calr 



l Obj[n] 



_Vis[i]. 



L Obj[n] 



W' T 



Obj[m] 



_UF[i]'calr- 



l Obj[m]' 



where frames Vis[i]' and UF[i]' ca i represent positions of frames Vis[i] and UF[i] ca i after object 
position Obj[n] changes to Obj[m]. 



Dfef_TCP=UF[i]cal 




Camera 



Obj[m] 



Obj[m] 



Fig. 7. Frame transformations in mobile-camera application 

With vision-determined UF l i ' cal TuF[i]'cai in Eq. (17) (for fixed-camera) or Eq. (20) (for 
mobilecamera), the robot programmer is able to apply Eq. (15) for shifting all pre-taught 
robot points PrnlHyr 1 ^' 1 ' into Pfn] 1 ™ 1 ,?^ 1 for the vision-identified object at position Obj[m] as 
shown in Eq. (21) 



P[l 



and 



, -|iUF[i]cal_UF[i]cal T 

t Il J UT[k] _ 1 UF[i]'cal 



xP[n] UT[k] 



pr n liUF[i]'cal_pr lUF[iIcal 
1 L X1 J UT[k] _ L t Il JuT[k] 



(21) 



Table 6 shows the FANUC TP program used in a fixed-camera FANUC vision application. 
The program calculates "vision offset" UF Pl cal TuF[i]'cai in Eq. (17), sends it to user-specified 
robot position register PR[x], and transforms robot point P[n]™S al m Eq. (21). 



Advanced Techniques of Industrial Robot Programming 



93 



FANUC TP Program 


Description 


1: R[l] = 0; 


Clear robot register R[l] which is 
used as the indicator for vision 
"Snap & Find" operation. 


2: VisLOC Snap & Find ('2d Single 1 , 2); 


Acquire vis ToBj[m] from snapshot 
view picture '2d single', find vision- 
measured offset ^M^TuFiii'cai, and 
send it to robot position register 
PR[1]. 


3: WAIT R[l] <> 0; 


Wait until the VisLOC vision 
system sets R[l] to '1' for a 
successful vision "Snap & Find" 
operation. 


4: IF R[l] <> 1, JMP LBL[99] 


Jump out of the program if the 
vision system cannot set R[l] as '1'. 


5: OFFSET CONDITION PR[1], UFRAME[i] ca i; 


Apply UF H cal TuF[i]'cai as Offset 
Condition. 


6: J P[n] 50% FINE OFFSET; 


Transforms robot point 

p [ n ]uT[kr IbyUF,iIcalTuF|ii ' cai - 



Table 6. FANUC TP program used in a fixed-camera FANUC vision application 



4. Creating Robot Points through Robot Simulation System 

With the today's robot simulation technology a robot programmer may also utilize the robot 
simulation software to program the motions and actions of a real robot offline in a virtual 
and interactive 3D design environment. Among many robot simulation software packages, 
the DELMIA Interactive Graphics Robot Instruction Program (IGRIP) provides the robot 
programmers with the most comprehensive and generic simulation functions, industrial 
robot models, CAD data translators, and robot program translators (Cheng, 2003; Connolly, 
2006). 

In IGRIP, a simulation design starts with building the 3D device models (or Device) based 
on the geometry, joints, kinematics of the corresponding real devices such as a robot and its 
peripheral equipment. The base frame B[i](x, y, z) of a retrieved Device defines its position 
in the simulation workcell (or Workcell). With all required Devices in the Workcell, the 
robot programmer is able to create virtual robot points called tag points and program the 
desired motions and actions of the robot Device and end-effector Device in robot simulation 
language. Executing the Device simulation programs allows the robot programmer to verify 
the performance of the robot Device in the Workcell. After the tag points are adjusted 
relative to the position of the corresponding robot in the real robot workcell through 
conducting the simulation calibration, the simulation robot program can be downloaded to 
the real robot controller for execution. Comparing to the conventional online robot 
programming, the true robot offline programming provides several advantages in terms of 
the improved robot workcell performance and reduced robot downtime. 
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4.1 Creation of Virtual Robot Points 

A tag point Tag[n] is created as a Cartesian frame and attached to the base frame B[i] of a 
user-selected Device in the Workcell. Mathematically, the Tag[n] position is measured in 
frame B[i] as frame transformation B l'] j and can be manipulated through functions of 

Selection, Translation, Rotation, and Snap. During robot simulation, the motion instruction 
in the robot simulation program is able to move frame Def_TCP (or UT[k]) of the robot 
Device to coincide a Tag[n] only if it is within the robot's workspace and not a robot's 
singularity. The procedures for creating and manipulating tag points in IGRIP are: 
Step 1. Create a tag path and attach it to frame B[i] of a selected Device. 
Step 2. Create tag points Tag[n] (for n = 1, 2, . . . m) one at a time in the created path. 
Step 3. Manipulate a Tag[n] in the Workcell. Besides manipulation functions of selection, 
translation, and/ or rotation, the "snap" function allows the programmer to place a 
Tag[n] to the vertex, edge, frame, curve, and surface of any Device in the Workcell. 
Constraints and options can also be set up for a specific snap function. For example, 
if the "center" option is chosen, a Tag[n] will be snapped on the "center" of the 
geometric entities such as line, edge, polygon, etc. If a Tag[n] is required to snap on 
"surface," the parameter "approach axis" must be set up to determine which axis of 
Tag[n] will be aligned with the surface normal vector. 

4.2 Accuracy Enhancement of Virtual Robot Points 

It is obvious that inevitable differences exist between the real robot wokcell and the 
simulated robot Workcell because of the manufacturing tolerance and dimension variation 
of the corresponding components. Therefore, it is not feasible to directly download tag point 
Tag[n] to the actual robot controller for execution. Instead, the robot programmer must 
apply the simulation calibration functions to adjust the tag points with respect to a number 
of robot points uploaded from the real robot workcell. The two commonly used calibration 
methods are calibrating frame UT[k] of a robot Device and calibrating frame B[i] of a Device 
that attaches Tag[n]. The underlying principles of these methods are the same with the 
design of robot UT and UF frames as introduced in section 2.1 and 2.2. For example, assume 
that the UT[k]' of the robot end-effector Device is not exactly the same with the UT[k] of the 
actual robot end-effector prior to UT[k] calibration. To determine and use the actual UT[k] 
in the simulation Workcell, the programmer needs to teach three non-collinear robot points 
through UT Frame Setup procedure in the real robot system and upload them into the 
simulation Workcell so that the simulation system is able to calculate the origin of UT[k] 
with the "three-point" method as described in Eq. (5) and Eq. (6) in section 2.1. With the 
calibrated UT[k] and the assumption that the robot Device is exactly the same as the real 
robot, the UT[k] position relative to the R frame ( R TjjT[k]) of a robot Device in the simulation 
Workcell is exactly the same as the corresponding one in the real robot workcell. Also, prior 
to frame B[i] calibration, the Tag[n] position relative to frame R of a robot Device ( R TTag[n]) 
may not be the same as the corresponding one in the real robot workcell. In this case, the 
Device that attaches Tag[n] serves as a "fixture" Device. Thus, the programmer may define a 
robot UF[i] frame by teaching (or create) three or six robot points (or tag points) on the 
features of the real "fixture" device (or "fixture" Device) in the real workcell (or the 
simulation Workcell). Coinciding the created UF tag points in the simulation Workcell with 
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the corresponding uploaded real robot points results in calibrating the position of frame B[i] 
of the "fixture" Device and the Tag[n] attached to it. 

5. Transferring Robot Points to Identical Robots 

In industrial robot applications, there are often the cases in which the robot programmer 
must be able to quickly and reliably change the existing robot points in the robot program so 
that they can be accurate to the slight changes of components in the existing or identical 
robot workcell. Different methods have been developed for measuring the dimensional 
difference of the similar components in the robot workcell and using it to convert the robot 
points in the existing robot programs. For example, as introduced in section 2.1 and 2.2, the 
robot programmer can measure the positional variations of two similar tool-tip points and 
workpieces in the real robot workcell through the offsets of UT[k] and UF[i], and 
compensate the pre-taught robot points with either the robot system utility function or the 
robot program instruction. However, if the dimensional difference exists between two 
identical robots, an external calibration system must be used for identifying the robots' 
difference so that the taught robot points P[n] for one robot system can be transferred to the 
identical one. The process is called the robot calibration, which consists of four steps (Cheng, 
2007; Motta et al, 2001). The first step is to teach specially defined robot points P[n]. The 
second step is to "physically" measure the taught P[n] with an appropriate external 
measurement device such as laser interferometry, stereo vision, or mechanical "string pull" 
devices, etc. The third step is to calculate the relevant actual parameters of the robot frames 
through a specific mathematical solution. 

The Dynalog DynaCal system is a complete robot calibration system that is able to identify 
the parameters of robot joint frames, UT[k], and UF[i] in two "identical" robot workcells, 
and compensate the existing robot points so that they can be download to the identical robot 
system for execution. Among its hardware components, the DynaCal measurement device 
defines its own measurement frame through a precise base adaptor mounted at an 
alignment point. It uses a high resolution, low inertia optical encoder to constantly measure 
the extension of the cable that is connected to the tool- tip point of the robot's end-effector 
through a DynaCal TCP adaptor, and sends the encoder measurements to the Window- 
based DynaCal software for the identification of the robot parameters. 

Prior to the robot calibration, the robot programmer needs to conduct the calibration 
experiment in which a developed robot calibration program moves the robot Def_TCP 
frame to a set of taught robot calibration points. Depending on the required accuracy, at 
least 30 calibration points are required. It is also important to select robot calibration points 
that are able to move each robot joint as much as possible in order to "excite" its calibration 
parameters. The dimensional difference of the robot joint parameters is then determined 
through a specific mathematical solution such as the standard non-linear least squares 
optimization. Theoretically, the existing robot kinematics model can be modified with the 
identified robot parameters. However, due to the difficulties in directly modifying the 
kinematic parameters of an actual robot controller, the external calibration system 
compensates the corresponding joint values of all robot points in the existing robot program 
by solving the robot's inverse kinematics equations with the identified robot joint 
parameters. 
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In DynaCal UT[k] calibration, the programmer needs to specify at least three non-collinear 
measurement points on the robot end-effector and input their locations relative to the 
desired tool-tip point in the DynaCal system during the DynaCal robot calibration. 
However, when only the UT[k] origin needs to be calibrated, one measurement point on the 
end-effector suffices and choosing the measurement point at the desired tool-tip point 
further simplifies the process because its location relative to the desired tool-tip point is then 
simply zero. In DynaCal UF[i] calibration, the programmer needs to mount the DynaCal 
measurement device at three (or four) non-collinear alignment points on a fixture during the 
DynaCal robot calibration. The position of each alignment point relative to the robot R 
frame is measured through the DynaCal cable and the TCP adaptor at the calibrated UT[k]. 
The DynaCal software uses the measurements to determine the transformation between the 
UF[i]Fix on the fixture and the robot R frame, denoted as R TuFp]Fix. With the identified values 
of frames UT[k] and UF[i]pi x in the original robot workcell and the values of UT[k]' and 
UF[i]'Fix in the "identical" robot workcell, offsets UF and UT can be determined and the 
robot points P[n] used in the original robot cell can be converted into the corresponding 
ones for the "identical" robot cell with the methods as introduced in sections 2.1 and 2.2. 



Robot i Robot' 




Fig. 8. Determining the offset of UF[i] in two identical robot workcells through robot 
calibration system 



The following frame transformation equations show the method for determining the robot 
offset UF P]'TuF[i] in two identical robot workcells through calibrated values of UF[i]Fi x and 
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UF[i]'pi x as shown in Fig. 8. Given that the coincidence of UF[i]pi x and UF[i]'pi x represents a 
commonly used calibration fixture in two "identical" robot workcells, the transformation 
between two robot base frames R' and R can be calculated in Eq. (22) 

''TR^W^TuFiflJ- 1 - ( 22 ) 

It is also possible to make transformation R 'TuF[i]' equal to transformation R TuF[i] as shown in 
Eq.(23) 

T UF[i]'= T UF[i]' ( 23 ) 

where frames UF[i] and UF[i]' are used for recording robot points P[n] and P[n]' in the two 
"identical" robot workcells, respectively. With Eq. (22) and Eq. (23), robot offset UF H'TuF[i] 
can be calculated in Eq. (24) 

UF[i] T _/R' T \-i x R T X R T . (24) 

1 UF[il _ V 1 UF[il7 x l R x l UF\U \ ' 



6. Conclusion 

Creating accurate robot points is an important task in robot programming. This chapter 
discussed the advanced techniques used in creating robot points for improving robot 
operation flexibility and reducing robot production downtime. The theory of robotics shows 
that an industrial robot system represents a robot point in both Cartesian coordinates and 
proper joint values. The concepts and procedures of designing accurate robot user tool 
frame UT[k] and robot user frame UF[i] are essential in teaching robot points. Depending on 
the selected UT[k] and UF[i], the Cartesian coordinates of a robot point may be different, but 
the joint values of a robot point always uniquely define the robot pose. Through teaching 
robot frames UT[k] and UF[i] and measuring their offsets, the robot programmer is able to 
shift the originally taught robot points for dealing with the position variations of the robot's 
end-effector and the workpiece. The similar method has also been successfully applied in 
the robot vision system, the robot simulation, and the robot calibration system. In an 
integrated robot vision system, the vision frame Vis[i] serves the role of frame UF[i]. The 
vision measurements to the vision-identified object obtained in either fixed-camera or 
mobile-camera applications are used for determining the offset of UF[i] for the robot system. 
In robot simulation, the virtual robot points created in the simulation robot workcell must be 
adjusted relative to the position of the robot in the real robot workcell. This task can be done 
by attaching the created virtual robot points to the base frame B[i] of the simulation device 
that serves the same role of UF[i]. With the uploaded real robot points, the virtual robot 
points can be adjusted with respect to the determined true frame B[i]. In a robot calibration 
system, the measuring device establishes frame UF[i] on a common fixture for the 
workpiece, and the measurement of UF[i] in the identical robot workcell are used to 
determine the offset of UF[i]. 
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1. Introduction 

Many current robotic applications are limited by the industry state of art of the 
manipulators control algorithms. The inclusion of force and vision feedbacks, the possibility 
of cooperation between two or more manipulators, the control of robots with irregular 
topology will certainly enlarge the industrial robotics applications. The development of 
control algorithms to this end brings the necessity of using open-architecture controllers. 
Generally the robotic controllers are developed for position control, without accomplishing 
integrally the requirements of tasks in which interactions with the environment occur. 
Therefore, this is currently one of the main research areas in robotics, e.g., in (Abele et al., 
2007) is presented the identification of characteristics to an industrial robot to execute 
machining applications. To consider this interaction the robot controller has to give priority 
to the force control time response, because in the instant of end-effectors contact with the 
surface, several forces act on the system. Depending on the speeds and the accelerations 
involved in the process, damages or errors can occur. To avoid these effects, compliances are 
inserted in tool or in surface of operation. 

A new reference model for a control system functional architecture applied to open- 
architecture robot controllers is presented. Where, this model is applied for integrally 
development of a five-layer based open-architecture robotic controller for interaction tasks, 
which uses parallel and distributed processing techniques, avoiding the necessity of 
compliance in system, allowing a real-time processing of the application and the total 
control of information. This architecture provides flexibility, the knowledge of all the 
control structures and allows the user to modify all controller layers. The used controller 
conception aims to fulfill with the following requirements: high capacity of processing, low 
cost, connectivity with other systems, availability for the remote access, easiness of 
maintenance, flexibility in the implementation, integration with a personal computer and 
programming in high level. 
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This chapter is organized as follows. Section 2 overviews the most relevant categories, 
definitions and requirements of robot controllers. Section 3 details the reference model for 
open-architecture controller development. Section 4 describes the robot retrofitting for 
interactions tasks. Section 5 presents and discusses the experimental setup. Finally, Section 6 
concludes the chapter and outlines future research and development directions. 

2. Open-architecture Robot Controllers 

Various open control architectures for industrial robots have already been developed by 
robot and control manufacturers as well as in research labs. In (Lippiello et al., 2007) is 
presented an open architecture for sensory feedback control of a dual-arm industrial robotic 
cell for cooperation tasks. In (Macchelli & Melchiorri, 2007) is presented a real-time control 
system based on RTAI-Linux operating system and developed for coupling of an advanced 
end-effector. (Hong et al., 2001) develop a system of robot open control based on a reference 
model OSACA. (Bona et al., 2001) propose a real-time architecture for robot control system 
development based in real-time operating system for embedded systems, RTOS. In (Donald 
& Dunlop, 2001) present a retrofitting of a path control system for a hydraulic robot based 
on a FPGA executing the embedded operating system RTSS. The inexistence of a standard 
methodology for architecture controller project difficult the development of high-openness 
degree control system. 

Most of the existing robot control open architectures are based on a standard PC hardware 
and a standard operating system, because I/O boards and communication boards for robots 
have a higher cost in relation to the similar boards for PCs. Another reason is the lack of 
standardization of robot peripherals, with each manufacturer developing its own protocols 
and interfaces, forcing the users to buy all the components of a single manufacturer (Lages 
et al., 2003). Additionally, a PC based controller can be integrated more easily with many 
commercially available add-on peripherals such as mass storage devices, Ethernet card and 
other I/O devices. So, the facility to integrate other functionalities is a strong reason to use a 
standard PC hardware in robot control open architectures. 

Another reason is that the robot programming languages are, at low level, more similar to 
the Assembly languages than to the modern high level languages and this may difficult 
implementations (Lages et al., 2003). In a PC based controller standard software 
development tools (e.g., Visual C++, Visual Basic or Delphi) can be used. 

2.1 Definitions 

The definition of open system, according to Technical Committee of Open Systems of IEEE 
is "An open system provides capabilities that enable properly implemented applications to 
run on a variety of platforms from multiple vendors, interoperate with other system 
applications and present a consistent styler of interaction with the user". A open- 
architecture control system has the capacity to operate with the best components of different 
manufacturers. What makes possible the easy integration of new system functionalities. 
From user point of view, the "openness" of the systems consists in capabilities to integrate, 
extend and reuse software modules in control systems (Lutz & Sperling, 1997). In (Pritschow 
& Altintas, 2001) and (Nacsa, 2001) the "degree of openness" of a system is defined by some 
criteria, as: 
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• Extendibility: A variable number of modules can be executed simultaneously in a same 
platform, without causing conflicts, i.e., this characteristic depends mainly on the 
operating system, that should accomplish a multi-task processing, and also of modules 
coupling level, that should allow those operations. 

• Interoperability: The modules work together efficiently and they can interchange data 
in a defined way through logical and physical communication buses. 

• Portability: The modules can be executed in different platforms without modifications, 
maintaining their functionalities, i.e., they should conform software and hardware 
standards to maintain the system compatibility with other platforms. 

• Scalability: Depending on the user requirements, the module functionalities and 
performance and size of the hardware, software and firmware can adapt for the system 
optimization. 

Those characteristics define the "degree of openness" of a system, how more extended and 
refined, major will be the level of openness. For open-architecture controllers, one more 
characteristic should be considered, the modularity. 

• Modularity: The system is divided in specialized subsystems, called modules, that can 
be substituted without significant modifications in system. This characteristic consists 
of logical and physical system decomposition in small functional units. 

2.2 Categories 

The controllers are characterized by the freedom of access information or simply for "degree 
of openness". Usually, the control of several system modules (e.g., unit power and low level 
control) is proprietary and cannot be modified by user, other levels are considered open 
(e.g., communication interface and high-level control), i.e., they are based on hardware and 
software standards with specifications of open interface. 

In (Pritschow & Altintas, 2001), (Lutz & Sperling, 1997) and (Ford, 1994), the "degree of 
openness" of a system is defined in agreement with access concept to controller layers, like 
this, they can be classified in three categories: 

• Proprietary: That system modality allows the access just to application layer, being 
therefore, a closed system. In those systems is extremely difficult or impossible the 
integration of external modules. 

• Hybrid or Restricted: That category makes available the access to application layer and 
a controlled access to operating system module. The operating system has a fixed 
topology, however, allows small changes in control system modules (e.g., gains and 
parameters). 

• Open: Open-architecture systems allow integral access of application layers and 
operating system modules, supplying a homogeneous vision of the system, allowing 
the manipulation and modification of all modules that compose the system. Its offers 
interchangeability, scalability, portability and interoperability. 

2.3 Requirements 

One of main requirements for a system to be characterized with open-architecture is the 
necessity of the control functionalities be subdivided in small functional units with a solid 
relationship among the subsystems. Consequently, the modularity becomes fundamental for 
a control system to have an open-architecture (Pritschow & Altintas, 2001). 
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The determination of module complexity should consider factors as the "degree of 
openness" wanted and integration cost. Small modules supply a high-level openness, but 
they increase the complexity and integration costs. A low modularity can drive for a high 
demand of resources and to deteriorate the system performance, not allowing real-time data 
articulation (Nacsa, 2001). 

The system structuring through a modular interaction requests a detailed group of 
relationship methods, composed by Application Programming Interfaces (i.e., these are a 
group of routines and software standards for extern access of their functionalities). In open 
control systems these interfaces need to be standardized (Pritschow & Altintas, 2001). 
The modular platforms encapsulate the operation system specific methods absorbing the 
hardware, operating system and communication characteristics. What promotes a high level 
data exchange, this abstraction requests a data mediation module, called middleware. These 
data concatenation and adaptation points increase the portability and interoperability of 
distributed applications in heterogeneous environments. 

3. The Reference Model for Open-Architecture Robot Controllers 

The reference model for a control system functional architecture presented in (Sciavicco & 
Siciliano, 2000) has a priority focus in the control structure, little exploring the other levels of 
robot controllers. 

This work proposes a new reference model for a control system functional architecture 
applied to open-architecture robot controller. The model is based on model of (Sciavicco & 
Siciliano, 2000), however, it expands the approach for all controller levels, adapts their 
layers in agreement with the standard ISO 7498-1 and considers the definition, categories, 
requirements and tendencies for open-architecture controllers. The structure of the 
proposed reference model is represented in Fig. 1, where the five hierarchical levels are 
illustrated. To proceed, those layers will be described individually. 

3.1 Task Layer 

The task layer is responsible for industrial robot control tasks grouped in three categories: 
trajectory planning, supervisory system and control law. Those operations are processed in 
the central equipment of the system, usually a personal computer (PC). In remote control 
operations, the operations can be divided in two software modules with relationship client- 
server. The trajectory planning and supervisory system will be processed with smaller time 
requirements in client, while the control structure will be processed in real time of 
application in server. 

3.2 Integration Layer 

The adopted functional architecture hierarchical structure, together with its articulation into 
different modules, suggests a hardware implementation that exploits distributed 
computational resources interconnected by means of suitable communication channels. At 
the integration layer, the information adaptation is accomplished (i.e., concatenation and 
organization) incoming from several processors that compose the distributed system. 
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These operations supply to superior layer a heterogeneity vision of the system to sharing 
resources. In this level, peripherals with high-level of abstraction (e.g., exteroceptive 
sensors) are also appropriate in this level. 

3.3 Communication Layer 

At the communication layer, the interconnection of information among the system 
processors is accomplished, usually using high-speed data transmission buses. The network 
topology is indifferent, however, it is important the use of redundant ways for connection 
among all intermediate points and the net central knot through the main bus and the 
embedded systems interconnection by an alternative communication bus. Every system 
interconnection accomplished in this layer is based in International Standard ISO/IEC 7498-1. 
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Fig. 1. Functional architecture of proposed reference model. 



3.4 Interface Layer 

The interface layer is composed by the embedded systems, i.e., dedicated hardware's to 
process specific task software (called firmware) encapsulated in internal storage memories. 
This organizational structure divides the system in small hardware modules and 
consequently, distributes the system processing. The processing distribution degree is 
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proportional at the utilization level of dedicated processors in system. The system 
decomposition in task dedicated processors guarantees a fixed and minimum response time. 

3.5 Physical Layer 

The industrial manipulator physical access (i.e., actuators and proprioceptive sensors) occur 
in physical layer, composed only by the input and output robot data channels. Usually, the 
actuator activation is realized indirectly, because, the controller signs only access the unit 
power that adapts this signs for the motors. 

4. Reference Model applied to Interaction Tasks 

Special requirement for robot controllers that includes force control 

Generally the robotic controllers are developed for applications that require only position 
control, and the robot end effector doesn't contact the workspace during its movement. The 
interaction concept is related with the contact between robot and environment, where 
generated force and torque profiles need to be controlled. In applications that need force 
control, the end effector contacts some surface in its workspace and this interaction 
generates contact forces that must be controlled in a way to fulfill the task correctly, without 
damaging both, robot tools and the working objects. 

The contact force intensities, originated by tool movements commanded by the robot 
controller, depend on both, the tool rigidity and the object surface rigidity, and they must be 
also controlled. A small tool movement could originate large force intensities in case the tool 
and the object surface rigidity are large. It should be noted that by introducing compliance 
to the tool we generate a delay in the application of the forces and this could be 
unacceptable in some applications. Consequently, the system should have a small time 
response to these forces, to prevent tool, robot or object damages. The use of high 
performance systems is a requisite of controllers for application of force control. 
Therefore, the reference model proposed was applied, considering the interaction tasks 
requirements, for retrofit of old industrial manipulator. The resultant functional structure 
for controller is presented in Fig. 2 and described as follows. 

4.1 Task Layer for interaction tasks 

The task layer has a mathematical environment prepared to make operations with matrices 
in which the control law is stored. The information proceeding from the n joints are 
available in matrices nxl corresponding to the position vector 5, and the velocity vector g, 
where the lines represent the joints. The force sensor data are stored in a matrix 6x1 
called i! = \£f /l' Js'fTjf^L'f'j]" ' which contains forces and moments data. The information to 
be directed to the motors and encoders is stored in an n x 3 control matrix -u. In this layer the 
user develops the control laws of position and/ or force of the manipulator and it is possible 
to carry through the task simulation. 
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Fig. 2. Functional architecture of proposed reference model applied to interaction tasks. 

4.2 Integration Layer for interaction tasks 

In the integration layer the concatenation and the organization of all the information coming 
from the sensors and to be sent to the superior layer are done. In case of the inclusion of a 
new hardware to the system, it is necessary to add its control structure to this layer. This is 
carried through by a high-level application that manages the power unit and control unit. 
Preventing any irregular movements and danger situations and controlling the components 
of the lower level. In this software the controller's components can be activated or disabled 
independently. 



4.3 Communication Layer for interaction tasks 

The communication layer controls the data transfer by managing the interface USB 
(Universal Serial Bus 2.0) and the industrial protocol CAN (Campus Area Network), both 
high performance communication devices. The USB makes a system interconnection 
through a star form topology, which has the computer as a central knot. Each USB door 
supports up to 127 devices and, in this manner, it is possible to connect a great quantity of 
joints to the controller. The protocol CAN form the bus between the secondary knots 
(motion controllers) and the result structure is a redundant net architecture. The 
implementation of this bus is still being explored and intends to introduce the possibility of 
a joint to access information of another joint without passing through the central knot. This 
will increase the performance of the net and it gives the opportunity to an implementation 
of the system of control without the central knot: a totally embedded control. The resultant 
architecture communication is presented in Fig. 3. 
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Fig. 3. Communication architecture for interaction tasks. 



4.4 Interface Layer for interaction tasks 

The interface layer comprises the embedded systems that carry out the control of the robotic 
joints, named motion controllers. Each of these motor digital controllers decodes the 
corresponding encoder signal and generates the modulation width pulse (PWM) to the 
control of the respective motor. Each of these systems has an optical isolated interface to 
prevent any inadequate return to the processor. It possesses a great amount of expansion 
doors, which allows the connection of other tools. 

We developed the controller with a modular architecture to have an independent control for 
each joint and so, divide the mathematical complexity among the processors of the system. 
This results in a distributed processing organized by the central knot (computer), where the 
operations occur in parallel. This methodology facilitates the expansion and maintenance of 
the system. 

Currently the system operates with a medium tax of update of the signals of 1 ms, only for a 
convention of literature. In case of necessity this largeness can be diminished. 

4.5 Physical Layer for interaction tasks 

The most inferior layer, here denominated physical layer, is the power unit of the motors 
and the angular position sensors. 



5. Experimental Environment 

The retrofitting methodology was validated with the adaptation of an old anthropomorphic 
manipulator, model Rvl5, produced by the REIS Robotics, for interaction tasks. Where was 
substituted the proprietary controller by the new open-architecture controller and coupled a 
force sensor in system. 
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The REIS Rvl5 robot has six rotating joints acted by electric motors and the angular 

positions measurement are done using incremental optical encoders. It is a manipulator 

with a topology that is very common in industry applications, which constitutes an 

anthropomorphous arm (joints 1, 2 and 3) with a spherical wrist (joints 4, 5 and 6). 

The Fig. 4 presents a complete diagram of the embedded five-layer open-architecture robotic 

controller for an industrial manipulator, containing it data flow and the systems 
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Fig. 4. Experimental environment for interaction tasks. 



5.1 Hardware architecture description 

The system's hardware was developed and built using high performance and reliability, low 
cost and easiness to be found in the market components. The Fig. 5 shows the diagram of 
internal blocks used in the motion controllers. 

The main component is a digital signal controller (DSC) produced by the Microchip 
Technology Inc. named dsPIC30F6010A. It operates with 16-bits, in a 120 MHz frequency 
with a package TQFP of 80 pins, and is one integrant of the family of the motors control. It 
possesses a great amount of well differentiated modules including an ample program 
memory with a 144K and a non-volatile memory with 4096 bytes for information storage. It 
has 16 ways for A/D conversions and the necessary modules of communication. For the 
communication through USB we used a component which carries through the conversion of 
module UART for the bus. This component supports transference taxes up to 3 Megabaud 
and is manufactured by the FTDI (Future Technology Devices International Ltd). 
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Fig. 5. Interface architecture for interaction tasks. 

Moreover, it possesses other functionalities, including the generation of a digital external 
signal oscillator with changeable frequencies. Besides this, the same manufacturer produces 
available Royalty-Free drivers for many operational systems, for this form of 
implementation. To implement the requirements for the physical layer defined by the ISO- 
11898, we connect the CAN industrial protocol to a transceiver of high speed, which 
supports until IMb/s. 

The system firmware implementation uses the high level language C. This is completely 
modulated and organized in units, to facilitate modifications. All the modules operate with 
interruptions of the processor with distinct priorities, such that an operation of less priority 
doesn't delay a higher importance process. 

The module of motion controller is also composed by a 16 bits PWM generator and by a 
module to read the quadrature encoder (named QEI), which we extend for 32 bits. 
Connected to it there is an optical decoupling barrier and an H-bridge for the control of the 
power unit that supports 100V and 8A. There are also amplified auxiliary output channels, 
which operate until 100V and 6A. To protect the system, the encoder entrances and the 
auxiliary inputs also have been connected to optical decoupling barrier. Internally there is 
still a great amount of resources that had not been used and that can be useful in future 
upgrades of the system. 

The experimental validation of the proposal was accomplished with implementation of a 
indirect force control strategy, the impedance control that will be presented to follow. 



5.2 Firmware architecture description 

The real time processing is obtained with the modularity of the embedded controller. These 
modules communicate only through hardware interrupts with eight priority levels. The 
software architecture and realistic physical modelling of the sensors and actuators provided 
to system a high response time. 
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The servo motors are controlled through a embedded self-tuning PID controller that uses 
the linear actuator dynamic model. The Fig. 6 presents the validation of dynamic model. 
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Fig. 6. Validation of the dynamic model behaviour. 



5.3 Software architecture description 

The software was developed using a high level object-oriented language (C++), for the 
Linux operating system recompiled for real-time application interface (RTAI). The control 
system monitors the processor activity, because most processes works through threads. 
Like this, when the processor activity reaches a critical level, the threads priorities are 
altered favouring controller essential tasks. 



5.4 Impedance control 

The manipulator control strategies for interaction tasks are grouped usually in two 
categories: indirect force control and direct force control. The first approaches the movement 
with an implicit force feedback only based in movement control, the other supplies the 
possibility of force control for a wanted value, through an explicit force feedback (Sciavicco 
&Siciliano, 2000). 

The classic impedance control approaches contact force indirectly modeling the interaction 
as a mass-spring-damper system. The indirect relationship is a consequence of force sign 
influence on control law, the objective is to adapt the manipulator dynamic behavior in 
contact with the environment and not to fulfill a position and/ or force trajectory. This way, 
an explicit force feedback doesn't exist in system, because, this signal just supplies the 
system impedance in contact with a surface. 

Therefore, the fundamental philosophy of impedance control, in agreement with (Hogan, 
1985), is that the control system regulates the manipulator impedance, that is defined by 
relationship between the speed and applied force, Fig. 7 (Zeng & Hemami, 1997). The 
formulation of this control strategy is presented to follow, in the equation 1. 
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Fig. 7. Impedance force control. 
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Where, fits, is the mass matrix, £i I* is the damping matrix, "f is the stiffness matrix and 

ft>A is the interaction force matrix. 

Equally to position control for inverse dynamics, where the impedance control is based, the 
integral knowledge of manipulator dynamics is admitted. In this way, the accurate 
knowledge of object elasticity characteristics or contact environment is not necessary in this 
control strategy (Yoshikawa, 2000). 



5.5 Results 

The implemented control strategy uses force feedback only to regulate the manipulator 
impedance, assuming that the manipulator is in contact with operation surface. In this way, 
when some force be detected the control law only will regulate the impedance to establish 
the system. 

The application used to validate the developed controller for interaction tasks is based in 
this characteristic of the impedance control, however, in this case, the end-effector isn't in 
contact with the surface. Therefore, the manipulator is immobile, admitting to be in wanted 
impedance profile, and when detects external force controls the system impedance (Fig. 8). 
The joint speed profiles generated in experiments are present in Fig. 9. 




Fig. 8. Interface architecture for interaction tasks. 
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Fig. 9. Force and joint velocities profile of the impedance control. 

The system validation experiment accomplished consists in a parallel manipulation on robot 
workspace limits. The force profiles, detected by JR3 force and moments sensor, only adapt 
manipulator dynamic behaviour with environment dynamic characteristics. In this way, a 
profile of force control is used to drive the joints velocity behaviour to a desired impedance 
profile. 



6. Conclusion 

In this work was considered a new reference model for a control system functional 
architecture applied to open-architecture robot controllers. The proposed approach was 
applied for integrally developing of a five-layer based open-architecture robotic controller 
considering interaction tasks. The architecture uses parallel and distributed processing 
techniques and circumvents the necessity of compliance in system, allowing a real-time 
processing of the application and the total information control. 

Old manipulator retrofitting considers the problem of including controllers with new 
functionalities as force control. The main characteristics of these systems are high-stiffness 
and position control. These characteristics restrict response time of the system. Therefore, an 
open-architecture system can be projected to operate in real-time. 

The proposed reference model for open-architecture robot controllers was experimentally 
validated including the implementation of an indirect force control strategy in the robot 
controller. Practical tests have shown the interest of the proposed architecture in terms of 
controller flexibility, costs and maintenance and high capacity of processing. 
This reference model clarifies the concept of robot controller and explains the internal 
modules that compose robot control unit. The system decomposition makes possible the 
optimization of internal modules for a specific task, e.g., interaction tasks. In this way, it is 
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possible to include new functionalities to the system, e.g., other feedback signals, new 
actuators or dedicated processors for a specific problem, e.g., resolution of redundancy or 
inverse kinematics. 

In the actual stage, the researchers have been focused on the theoretical aspects of the 
problem. Further works will consider the model validation and experimental applications. 
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1. Introduction 

The shifting manufacturing requirements to high flexibility and short production cycle have 
urged the emerging of human-robot collaborative type of manufacturing systems. Human- 
robot collaboration is a dream combination of human flexibility and machine efficiency. 
However, in order to materialize this paradigm shift in manufacturing history, the 
interaction between human and robot in the perspective of collaborative operation has to be 
fully investigated. Many studies had been conducted in the area of human-robot 
collaboration in manufacturing (Kosuge et al., 1994; Oborski, 2004). Modeling techniques 
(Rudas & Horvath, 1996) provide an initial step to study on this collaboration relationship 
even before system development. To ensure a more human-centered solution, task analysis 
is adapted for the modeling approach in this study. The purpose of this work is to develop a 
modeling framework based on task analysis approach to assist human-robot collaboration 
planning in manufacturing systems. 

The entire development of this work is illustrated in a modeling development of an actual 
cable harness assembly in a prototype cellular manufacturing system (Duan et al., 2008). The 
outline of the paper is arranged as the following: Section 2 provides the literature reviews on 
human-robot collaboration in manufacturing and the overview of the prototype cellular 
manufacturing system setup together with the assigned cable harness assembly operation. 
Section 3 presents entire development of collaboration planning by task analysis including 
the brief introduction on task analysis approach, task decomposition by hierarchical task 
analysis, and collaboration analysis. Section 4 discusses the design enhancements by the 
modeling framework in operation process design and further extensions in human skill 
analysis, safety assessment and operation support. The modeling design is implemented in a 
prototype production cell to perform model validation and operation performance 
evaluation as illustrated in Section 5. Section 6 concludes the work and states the 
suggestions for future work. 
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2. Human-Robot Collaboration in Manufacturing 

2.1 Human-Robot Collaboration 

Many efforts had been contributed in the study on human-robot interaction. Agah had 
presented a general taxonomy on human interaction with intelligent systems (Agah, 2000). 
In manufacturing environment, Stahre had discussed several human-robot interaction 
problems (Stahre, 1995). Over the years, there are many proposals on robotic human 
operator assistance: Robot Assistant rob@work (Helms et al., 2002), COBOT (Colgate et al., 
1996), KAMRO (Karlsuhe Autonomous Mobile Robot) (Laengle et al., 1997), CORA 
(Cooperative Robotic Assistant) (Iossifidis et al., 2002), Humanoid Service Robot HERMES 
(Bischoff, 2001) and The Manufacturing Assistant (Stopp et al., 2002). Although much work 
had been conducted on human-robot interaction, the view point of this work is quite 
deviated from the common goal of these studies. The ultimate aim of this work is to 
improve manufacturing systems by effective human-robot collaboration, rather than how 
much 'social' between human and robot. Therefore, manufacturing requirements become 
the main criterion in the collaboration planning. On the other hand, conventional assembly 
planning focuses on simplifying assembly process for automation. The lack in addressing 
human-robot collaboration in design for assembly principles has motivated this work to 
develop a design approach to address human-robot collaboration in assembly planning. 

2.2 Practical Development in Cellular Manufacturing System 

In order to ensure practicability of this work, the entire development is linked on an actual 
cable harness assembly system in cellular manufacturing. Also known as cell production, 
cellular manufacturing is a human-centered production system that catered for complex and 
flexible assembly requirements (Isa, K. & Tsuru, 2002). The prototype production cell design 
in this project is shown in Fig. 1 (Duan et al., 2008). 




~^j Markin 



g Board 



Fig. 1. Prototype production cell design for cellular manufacturing 
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In this cellular manufacturing system, a mobile twin robot manipulators system is assigned 
to collaborate with a human operator to conduct a cable harness assembly operation (Fig. 2). 
The robot manipulators system is able to navigate itself within the production cell (between 
the parts rack and the workbench) and to facilitate collaborative assembly operations on the 
workbench. The human operator conducts the assembly operation in sitting position and 
uses the input switches to control the progress of the operation. The workbench is 
incorporated with a liquid crystal display television (LCD TV) to provide multimedia 
assembly information to the human operator. Additional position information is indicated 
by the laser pointer system. More detailed descriptions on the prototype production cell are 
available in Duan's work (Duan et al., 2008). 

The completed cable harness assembly is shown in Fig. 2. The human operator assembles 
components from the parts kit onto the marking board to form the product. The required 
tasks in one assembly includes cable insertion on connector and terminal, tape marking and 
cable tie binding, and the assembly of metal plate. This assembly process will be discussed 
further in the following section for collaboration planning. 




Fig. 2. Cable harness assembly 



3. Collaboration Planning by Task Analysis 

3.1 Task Analysis 

The main challenge in human-robot collaboration study is the complexity of human nature 
because normal mathematical computer modeling techniques are difficult to study on the 
behavior. Many research studies developed the collaboration modeling from the 'machine' 
point of view (Kosuge et al., 1998; Mizoguchi et al., 1999) resulting 'machine-driven' 
collaboration. Therefore, with the motivation to develop a more 'human-centered' 
collaboration in production system, this work has adopted task analysis method, which 
provides a more 'natural' way to define and study on human activities. Task analysis is a 
widely used scientific methodology to model human task in various ergonomics and human 
factors studies (Hodgkinson & Crawshaw, 1985), medical surgery (Sarker et al., 2008), error 
prediction (Lane et al., 2006), and software interface design (Mills, 2007; Richardson et al., 
1998). The main advantage of task analysis is the ability to describe human activities with 
'abstract descriptions'. This temporal abstraction (Killich et al., 1999) is very useful in human- 
robot collaboration modeling especially when the actual optimal sequence of activities is yet 
to be defined. In task analysis development, the task is defined as goal and the required 
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activities (sub goals) that must be carried out to achieve it (Annett & Duncan, 1967; 
Hollnagel, 2006), and continuous branch out in sub goals to form a hierarchical tree. This 
hierarchical task analysis (HTA) approach (Kirwan & Ainsworth, 1992; Shepherd, 1998; 
Stanton, 2006) is adapted in this study to extend its capability to address human-robot 
collaboration in production systems. In the following discussion, the cable harness assembly 
operation is being decomposed into hierarchical assembly tasks tree to enable further 
investigation on the collaboration between human and robot in the collaborative operation. 

3.2 Task Decomposition by Hierarchical Task Analysis 

Fig. 3 shows the general operation flow of the cable harness assembly. The whole operation 
consists of mainly five different tasks. The first task is parts kit preparation, which is to 
gather all the required assembly components in the parts kit. The assembly begins with 
cable insertion to the connector in second task. In third task, the cables are being arranged 
on the marking board and bond with marking tape and cable tie. The purpose of the 
marking board is as a guide for the cables and assembly positions identifications. In the 
fourth task, the other ends of the cables are then inserted into the terminal. The final task is 
the metal plate assembly. 



Task 1 Parts kit preparation 



i 



Task 2 Cable assembly on connector 

Task 3 



I 



Cable arrangement on marking board 



i 



Task 4 



Cable assembly on terminal 



i 



Task 5 Metal plate assembly 



Fig. 3. General operation flow of the cable harness assembly 

Referring to HTA development guideline by Stanton (Stanton, 2006), the entire cable harness 
assembly is being decomposed into hierarchical task tree (Tan et al., 2008a). The overall 
operation objective is set as the main goal followed by general tasks in the assembly plan 
level as the sub goals. Then, on each sub goals, the decomposition is further branched out 
into control plan level. Table 1 summarizes the decomposition of the cable harness assembly 
into a HTA table. 'Assemble cable harness' (Super-ordinate 0) is the main goal of the entire 
operation. Based on the general operation flow in Fig. 3, the first hierarchical level of sub 
goals, 'Prepare parts kit', 'Assemble cables on connector , 'Arrange cables on marking board' ' , 
'Assemble cables on terminal' , and 'Assemble metal plate' (Super-ordinate 1, 2, 3, 4, and 5) are the 
general assembly tasks needed to achieve the main goal. The decompositions continue from 
the first level sub goals into another two hierarchical levels lower until all the task 
components are all well defined, as considered 'fit-for-purpose' (Stanton, 2006). In all the 
task levels, the execution sequence of the corresponding hierarchical level is defined in Plan 
components. With the developed HTA table, the entire cable harness assembly operation is 
well defined in a hierarchical task tree form for further development on collaboration 
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planning. The HTA table can be represented in a graphical form for better visualization 
illustrated in the next section. 



Super-ordinate 


Task components - Operation or Plan 





Assemble cable harness 

Plan 0: Do 1 then 2 then 3 then 4 then 5 then exit 


1. Prepare parts kit 

2. Assembly cables on connector 

3. Arrange cables on marking board 

4. Assemble cables on terminal 

5. Assemble metal plate 


1 


Prepare parts kit 

Plan 1: Repeat 1.1 then 1.2 for three parts then exit 


1.1 Arrange parts into tray 

1.2 Check parts // 


1.1 


Arrange parts into tray 

Plan 1.1: Do 1.1.1 then 1.1.2 then exit 


1.1.1 Retrieve part container // 

1.1.2 Grab part from container // 


2 


Assemble cables on connector 

Plan 2: Repeat 2.1 then 2.2 for two cables then 2.3 then exit 


2.1 Secure cable contacts on connector 

2.2 Temporary fix cable ends // 

2.3 Set connector on marking board 


2.1 


Secure cable contacts on connector 

Plan 2.1: Repeat 2.1.1 then 2.1.2 then 2.1.3 for two cables then exit 


2.1.1 Get cable from cable kit // 

2.1.2 Hold and locate insertion point // 

2.1.3 Insert cable contact into connector with driver // 


2.3 


Set connector on marking board 

Plan 2.3: Do 2.3.1 then 2.3.2 then exit 


2.3.1 Release connector // 

2.3.2 Get and place connector on marked location // 


3 


Arrange cables on marking board 

Plan 3: Do 3.1 for two cables then 3.2 for two marking tapes then 3.3 for two 

cable ties then exit 


3.1 Form cables on marking board 

3.2 Paste marking tape on cables 

3.3 Fasten cables with cable tie 


3.1 


Form cables on marking board 

Plan 3.1: Do 3.1.1 then 3.1.2 then exit 


3.1.1 Arrange cables along marked track // 

3.1.2 Fasten cable ends // 


3.2 


Paste marking tape on cables 

Plan 3.2: Repeat 3.2.1 then 3.2.2 for two marked locations then exit 


3.2.1 Get marking tape // 

3.2.2 Paste marking tape on marked location // 


3.3 


Fasten cables with cable tie 

Plan 3.3: Repeat 3.3.1 then 3.3.2 for two marked locations then exit 
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3.3.1 Get cable tie // 

3.3.2 Fasten cable tie on marked location // 


4 


Assemble cables on terminal 

Plan 4: Do 4.1 for two cables then 4.2 then exit 


4.1 Secure cable ends on terminal 

4.2 Set terminal on marking board 


4.1 


Secure cable ends on terminal 

Plan 4.1: Do 4.1.1 then repeat 4.1.2 then 4.1.3 for two cables then exit 


4.1.1 Get terminal from part tray // 

4.1.2 Hold and locate insertion point / / 

4.1.3 Insert cable end into terminal with driver // 


4.2 


Set terminal on marking board 

Plan 4.2: Do 4.2.1 then 4.2.2 then exit 


4.2.1 Release terminal 

4.2.2 Get and place terminal on marking board // 


5 


Assemble metal plate 

Plan 5: Do 5.1 then 5.2 then exit 


5.1 Secure cables on metal plate 

5.2 Set metal plate on marking board 


5.1 


Secure cables on metal plate 

Plan 5.1: Do 5.1.1 then repeat 5.1.2 then 5.1.3 then exit 


5.1.1 Get metal plate from part tray // 

5.1.2 Hold metal plate // 

5.1.3 Fasten cables on metal plate with cable tie // 


5.2 


Set metal plate on marking board 
Plan 5.2: Do 5.2.1 then 5.2.2 then exit 


5.2.1 Release metal plate // 

5.2.2 Get and place metal plate on marking board / / 



Table 1. HTA table of the cable harness assembly 



3.3 Collaboration Analysis 

The above task decomposition development based on HTA guideline has provided a coarse 
task outline of the cable harness assembly. The next step is to conduct detailed analysis for 
collaboration planning in task level. The analysis can be done in two stages, qualitative and 
quantitative, based on the complexity to determine the optimum collaboration solution for a 
given task. In qualitative analysis, the performance requirements of the task are compared 
qualitatively with the capabilities of human and robot to identify possible collaboration 
solution. If the optimum solution is not apparent, quantitative analysis can be conducted to 
score the possible solutions based on the performance requirements. 



Qualitative Analysis for Collaboration Task Identification. In qualitative analysis for 
collaboration task identification, the possible collaboration solution for each task is 
identified based on the comparison of the strength of human operator and robot 
manipulator with respect to performance requirements. Together with the definitions by 
Helms et al. on four types of human-robot cooperation in industrial environment: 
independent operation, synchronized cooperation, simultaneous cooperation, and assisted cooperation 
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(Helms et al., 2002), the collaboration tasks are identified and summarized in Table 2 for the 
first hierarchical level assembly tasks in cable harness assembly. 

The objective of Task 1, 'Prepare parts kit' (Super-ordinate 1) is to gather and arrange the 
assembly components into parts kit. This objective can be achieved easily by robot system 
using bin picking technique. Hence, it is suitable to be assigned to robot system for higher 
efficiency. Task 2, 'Assemble cables on connector' (Super-ordinate 2) requires handling of 
flexible cables for assembly. Therefore, human operator's flexibility is needed in this task. 
However, based on previous study (Pongthanya et al., 2008), the mental workload for the 
human operator to search for the correct insertion holes from the multi-holes connector can 
be relatively high and time consuming. Therefore, a possible collaboration by using robot 
system to indicate cable insertion holes by holding the connector under a fixed beam from 
the laser pointer might be a good solution. However, further quantitative analysis might be 
needed to justify this collaboration proposal. 'Arrange cables on marking board' in Task 3 
(Super-ordinate 3) requires handling of cables, marking tape and cable tie. Hence, these 
highly flexible operations are suitable to be assigned to human operator. Task 4, 'Assembly 
cables on terminal' (Super-ordinate 4) has the similar job requirements as in Task 2. Therefore, 
same collaboration solution might be applied. Task 5, 'Assembly metal plate' (Super-ordinate 
5) involves operation to fasten the cables on the metal plate with cable ties. A possible 
collaboration solution might be proposed, which the robot system can help to hold the metal 
plate to allow the human operator to use both hands to fasten the cables with cable ties. 



Super-ordinate 


Task components 


Collaboration 


1 


Prepare parts kit 

1.1 Arrange parts into tray 

1.2 Check parts // 


Independent operation by robot 
manipulators to prepare the parts 
kit 


2 


Assemble cables on connector 


Assisted cooperation by robot 
manipulator to hold the connector 
and indicate assembly points while 
human operator inserts the cable 
contacts 


2.1 Secure cable contacts on connector 

2.2 Temporarily fix cable ends / / 

2.3 Set connector on marking board 


3 


Arrange cables on marking board 

3.1 Form cables on marking board 

3.2 Paste marking tape on cables 

3.3 Fasten cables with cable tie 


Independent operation by human 
operator due to the requirement to 
handle flexible cables 


4 


Assemble cables on terminal 


Assisted cooperation by robot 
manipulator to hold the terminal 
and indicate assembly points while 
human operator inserts the cable 
ends 


4.1 Secure cable ends on terminal 

4.2 Set terminal on marking board 


5 


Assemble metal plate 


Assisted cooperation by robot 
manipulator to hold the metal 
plate while human operator 
fastens the cables with cable ties 


5.1 Secure cables on metal plate 

5.2 Set metal plate on marking board 



Table 2. Collaboration identification from the HTA table 



Quantitative Analysis by Analytic Hierarchy Process (AHP). When multiple requirements 
(productivity, fatigue, safety, etc.) and solutions (human system, robot system, human-robot 
system, etc.) are available for a given task and the optimum solution might not be apparent 
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by qualitative analysis, collaboration analysis by Analytic Hierarchy Process (AHP) [Saaty, 
2008; Saaty, 1994] approach can be conducted to assess the task quantitatively. Taking Task 
2, 'Assemble cables on connector (Super-ordinate 2) as example, the following description 
illustrates the quantitative analysis by AHP to verify the selection of human-robot 
collaborative system over human only system for the given task. 

Four performance requirements, namely, productivity (assembly duration), quality 
(assembly error), human fatigue (human operator tiredness), and safety (human operation 
safety), are set as the criteria in the AHP analysis. The evaluation is done based on 
comparison between human only system and human-robot system as alternatives (fully 
automated system is less practical to be considered due to the complexity of flexible cable 
handling in this task). Fig. 4 shows the AHP model of Task 2. In order to compute the 
priorities (relative weight of the nodes) of criteria and alternatives, pairwise comparison 
matrix of criteria (Table 3), pairwise comparison matrix of alternatives with respect to 
productivity (Table 4), quality (Table 5), human fatigue (Table 6), and safety (Table 7) are 
developed from the analysis. Based on the fundamental scale of pairwise comparisons 
(Harker & Vargas, 1987), the intensity of importance values are assigned to the pairwise 
comparison matrixes by comparing the importance between two criteria. The priorities are 
then being calculated by summing each row and dividing each by the total sum of all the 
rows in the corresponding matrix. 



Assemble Cables on Connector 
1.000 




Fig. 4. AHP model of Task 2 



From Table 3, the productivity and quality have the same importance (intensity of 
importance = 1) in achieving the assembly operation (goal). The safety criterion has been 
given higher priority in the pairwise comparison (intensity of importance = 2) over 
productivity and quality due to the high risk nature of the close range collaboration. The 
intensity of importance of productivity and quality over human fatigue also has been set to 2 
to put more focus on mental stress of the human operator during close range collaboration 
with the robot system. The safety has much stronger importance (intensity of importance = 
6) over human fatigue. The pairwise comparisons on the alternative systems with respect to 
each criterion are being judged based on the actual system performance. The improvements 
from human-robot design can be given a greater importance in the productivity (Table 4) 
and quality (Table 5). The assistance from robots also greatly reduced the workload burden 
of the human operator (Table 6). However, due to the close range collaboration, the safety 



Collaboration Planning by Task Analysis in Human-Robot Collaborative Manufacturing System 



121 



level is much lower in human-robot design (Table 7). The final priorities obtained for human 
system is 0.4681 and human-robot system is 0.5319 (Fig. 4). These have proven that human- 
robot system is much preferred solution that fit well to the performance criteria. 





Productivity 


Qualit 

y 


Human 
Fatigue 


Safety 


Priorities 


Productivity 


1 


i 


2 


1/2 


0.2030 


Quality 


1 


i 


2 


1/2 


0.2030 


Human Fatigue 


1/2 


1/2 


1 


1/6 


0.0977 


Safety 


2 


2 


6 


1 


0.4962 



Table 3. Pairwise comparison matrix of the performance criteria with respect to the goal 





Human 


Human-Robot 


Local 
Priorities 


Global 
Priorities 


Human 


1 


1/9 


0.1 


0.0203 


Human-Robot 


9 


1 


0.9 


0.1827 



Table 4. Pairwise comparison matrix of the alternative systems with respect to productivity 





Human 


Human-Robot 


Local 
Priorities 


Global 
Priorities 


Human 


1 


1/9 


0.1 


0.0203 


Human-Robot 


9 


1 


0.9 


0.1827 



with respect to quality 





Human 


Human-Robot 


Local 
Priorities 


Global 
Priorities 


Human 


1 


1/6 


0.1429 


0.0014 


Human-Robot 


6 


1 


0.8571 


0.0838 



Table 6. Pairwise comparison matrix of the alternative systems with respect to human 
fatigue 





Human 


Human-Robot 


Local 
Priorities 


Global 
Prioritie 

s 


Human 


1 


5 


0.8333 


0.4135 


Human-Robot 


1/5 


1 


0.1667 


0.0827 



Table 7. Pairwise comparison matrix of the alternative systems with respect to safety 



Collaboration Role Assignment. After the collaboration solution for each task in the first 
hierarchical level has been identified and justified, collaboration roles (Human-Robot, Human, 
and Robot) can be assigned to lower hierarchical task components (Table 8). The assignment 
can greatly assist the later control plan developments, for instance, robot system 
programming or human operator operation support development. The task modeling with 
collaboration planning is then can be completed in the task modeling tool developed in this 
project (Tan et al., 2009b) with color task role indicators for collaboration relationship 
visualization as shown in the graphical task model in Fig. 5. 
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Fig. 5. Task model (partially) of a cable harness assembly (human-robot: blue; human: pink; 
robot: green) 



Super-ordinate 


Task components 


Collaboration Roles 





Assemble cable harness 


Human-Robot 


1 

1.1 

1.1.1 

1.1.2 
1.2 


Prepare parts kit 
Arrange parts into tray 
Retrieve part container 
Grab part from container 
Check parts 


Robot 
Robot 
Robot 
Robot 
Robot 


2 

2.1 

2.1.1 

2.1.2 

2.1.3 

2.2 

2.3 

2.3.1 

2.3.2 


Assemble cables on connector 

Secure cable contacts on connector 

Get cable from cable kit 

Hold and locate insertion point 

Insert cable contact into connector with driver 

Temporarily fix cable ends 

Set connector on marking board 

Release connector 

Get and place connector on marked location 


Human-Robot 

Human-Robot 

Human 

Robot 

Human 

Human 

Human-Robot 

Robot 

Human 


3 

3.1 

3.1.1 

3.1.2 

3.2 

3.2.1 

3.2.2 

3.3 

3.3.1 

3.3.2 


Arrange cables on marking board 

Form cables on marking board 

Arrange cables along marked track 

Fasten cable ends 

Paste marking tape on cables 

Get marking tape 

Paste marking tape on marked location 

Fasten cables with cable tie 

Get cable tie 

Fasten cable tie on marked location 


Human 
Human 
Human 
Human 
Human 
Human 
Human 
Human 
Human 
Human 


4 

4.1 

4.1.1 

4.1.2 

4.1.3 

4.2 


Assemble cables on terminal 

Secure cable ends on terminal 

Get terminal from part tray 

Hold and locate insertion point 

Insert cable end into terminal with driver 

Set terminal on marking board 


Human-Robot 

Human-Robot 

Robot 

Robot 

Human 

Human-Robot 
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4.2.1 


Release terminal 


Robot 


4.2.2 


Get and place terminal on marking board 


Human 


5 


Assemble metal plate 


Human-Robot 


5.1 


Secure cables on metal plate 


Human-Robot 


5.1.1 


Get metal plate from part tray 


Robot 


5.1.2 


Hold metal plate 


Robot 


5.1.3 


Fasten cables on metal plate with cable tie 


Human 


5.2 


Set metal plate on marking board 


Human-Robot 


5.2.1 


Release metal plate 


Robot 


5.2.2 


Get and place metal plate on marking board 


Human 



Table 8. Collaboration role assignments 

4. Design Enhancements in Collaboration Planning 

4.1 Operation Process Design in Collaboration Planning 

From the task analysis in task model, possible collaboration operations can be identified in 
the assembly level. The collaboration analysis is then further continued into control level to 
assign the collaboration roles between the working agents in operation. The combination of 
original Plan components and the added collaboration role assignments has represented the 
collaboration assembly sequences. The developed task model is used in the assembly and 
task planning (Barnes et al., 1997; Gottschlich et al., 2002) as well as feasible assembly 
sequence generations and sequence changes. The following discussion will illustrate the 
improvements of operation process planning by this work (Tan et al., 2008b) in cable 
harness assembly. 

In the original human only setup of the cable harness assembly, Task 2, 'Assemble cables on 
connector requires the human operator to identify the specific insertion hole from a 12x6 
holes connector to insert the cable contact. If each operation consists of five sets of cables 
with a total of 5x2 cable contacts, this task can be highly mentally demanding and often 
causes error insertions especially after a long period of working (Pongthanya et al., 2008). 
Fig. 6 shows the original assembly operation sequence from Task 2 to Task 3.1. 

<— 



Get cable from cable kit 



I 



Hold and locate insertion point 



I 



Insert cable contact into connector with driver 



I 



Repeat for five 
cable sets 



Arrange cable along marked track 



I 



Fasten cable end 



Fig. 6. The initial assembly operation sequence from Task 2 to Task 3.1 



Robot system was introduced into the operation to assist the assembly. Without 
collaboration planning in this work, the robot system was assigned to assist human operator 
by holding the connector and locating the insertion point. Fig. 7 shows the modified 
assembly operation sequence after adding the assistance of robot system. 
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Get cable from cable kit [Human] 



4- 



<r 



Hold and locate insertion point [Robot] 



I 



Insert cable contact into connector with driver [Human] 



4. 



Repeat for five 
cable sets 



Arrange cable along marked track [Human] 



4- 



Fasten cable end [Human] 



I 



Release connector [Robot] 



Fig. 7. Modified assembly operation sequence with robot manipulator 

One of the most apparent collaboration issues from the above system was the looseness of 
the cables (or even being pull out from the cable fix) after the release of connector by the 
robot system at the end of the operation. By redesigning the operation sequence might be 
able to provide a solution for the issue but direct approach to revise the operation sequence 
planning is absence without proper task analysis. 

From the task analysis and collaboration planning in this work, the cable harness assembly 
is being decomposed into task sequence and role assigning between human operator and 
robot system as explained in previous section. Based on the task model (as summarized in 
Fig. 8), 'Arrange cable along marked track' and 'Fasten cable end' , are identified as repetitive 
steps and can be grouped into two single operation steps outside the operation loop. These 
steps can be simplified by adding a short step, 'Temporary fix cable end' (Fig. 9) in the 
operation loop. By placing the two steps at the end of the operation, it will also solve the 
previous 'loosen cable' issue by fasten the cables after the release of the connector. With this, 
the operation sequence is being improved while the human-robot collaboration is preserved. 



Get cable from cable kit [Human] 



4, 



<r 



Hold and locate insertion point [Robot] 



I 



Insert cable contact into connector with driver [Human] 



Repeat for five 
cable sets 



I 



Temporary fix cable end [Human] 



I 



Release connector [Robot] 



I 



Arrange cables along marked tracks [Human] 



I 



Fasten cable ends [Human] 



Fig. 8. Assembly operation sequence with task analysis 



From the above discussion, the design enhancements in term of (a) group repetitive steps 
('Arrange cable along marked track' and 'Fasten cable end'), (b) add interval step ('Temporary fix 
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cable end'), and (c) preserve collaboration are achieved by task analysis in collaboration 
planning. On the other hand, by observing the first hierarchical level of tasks and plan in 
task modeling (Fig. 10), the precedence relationships among the tasks are well defined (red 
dotted arrows) to assist possible assembly sequence changes while preserving the assembly 
process. In the cable harness assembly, Task 3 'Arrange cables on marking board' and Task 4 
'Assemble cables on terminal' are independent from each other after Task 2. Hence assembly 
sequence change is possible in switching the two tasks in the operation flow (Fig. 11). This 
assembly sequence changed operation is validated by the design implementation in the 
prototype production cell on the next section. 




Fig. 9. Temporary cable end fixing on cable fix 




-Lionnector fl- 
ssemblv.tmf 



□ 



Fig. 10. The first hierarchical level of tasks and plan with precedence relationships (red 
dotted arrows) 
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Prepare parts kit 
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Task 2 


Assemble cables on connector 
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Task 4 


Assemble cables on terminal 




I 


Task 3 


Arrange cables on marking board 




I 


Task 5 


Assemble metal plate 



Fig. 11. Cable harness assembly sequence change flow 

4.2 Design Extensions for Human Skill Analysis, Safety Assessment and Operation 
Support 

The main aim of this work is to enable collaboration planning for human-robot system in 
manufacturing. By adopting a more human-centered approach, task analysis enables 
detailed study on production operation to model and design the collaboration process. The 
developed task model provides several more extensions to further enhance the collaboration. 

Human Skill Analysis. In human operation skill study (Duan, 2009), modeling of operation 
in well structured task model enables further analysis on operator's cognition and motor 
skill requirements in the corresponding tasks. From the task model of cable harness 
assembly, potential cognition and motor skills can be extracted for the corresponding tasks 
(Table 9) in order to evaluate the effective skills for skill transfer. The purpose of skill 
transfer is to provide support especially to novice operators to improve working 
performance and collaboration process. 
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- Grasp cable's head 
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driver 
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- Memorize order of holes in 
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Arrange cables 
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3.1.2 


Fasten cable ends 


- Focus on cross route 


- Hold cables while fasten 






- Focus on positions of cable 
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- Tightly cross cables on cable 






- Focus on force feedback 


fixes 



Table 9. Potential cognition and motor skills for Task 2 and Task 3 
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Safety Assessment. Safety is the top most priority in human-robot systems. In the task 
modeling, safety assessment can be started as early as in the design stage. Task modeling 
provides a detailed analysis on human operations until lower control level to identify 
potential operation risk for the collaboration process. From the task model of cable harness 
assembly, it identifies possible human-robot collaboration in Task 2, Task 4 and Task 5. 
From the lower control tasks, two potential hazards can be indentified: (a) human operator's 
hands and/ or head being trapped by robot gripper, (b) collision of robot gripper with the 
human operator. Table 10 shows the risk assessment on these two potential hazards in 
collaboration (Tan et al., 2009a) based on industrial standards ANSI/RIA R15.06 (ANSI/RIA, 
1999) with reference to ISO 14121 (JIS B9702), ISO 13849-1 (JIS B9705-1), and BS EN 954-1. 
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Table 10. Risk assessment on cable harness assembly (Task 2, Task 4 and Task 5) 

Operation Support. One unique development in this work is to support operation by 
providing information to the human operator. Due to the shifting operation support from 
physical support, which is mainly taken care by automation, information support is one of 
the important factors that determine operator's working performance. In the prototype 
production cell for cable harness assembly in this work, a multimodal information support 
system (Duan et al., 2008) is developed as a man-machine interface for the human-robot 
collaboration system. However, in order to ensure effective information support, the content 
of the information has to be appropriate and relevance to the operation. Task modeling in 
this work has the function to extract and manage the information together with the task 
model. A task modeling editor (Tan et al., 2009b) (Fig. 12) is built on IBM Task Modeler 
Version 6 as the development environment of task modeling. An operation properties system 
is developed to encapsulate relevance information to the task according to the modeling 
levels and support requirements. Table 11 shows the basic operation properties in the task 
modeling for the support information development to support the assembly operations in 
the prototype production cell. 
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Tool 
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Operation Duration 


Time needed for the operation 
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Precedence relationship 


Information Support 


Operation Reference 
Media 


Reference description in any 
media format 


Safety 


Safety information 



Table 11. Operation properties in task modeling 




Fig. 12. Task modeling editor user interface 



5. Design Implementation and Operation Performance Evaluation 

An actual prototype production cell for cable harness assembly (Fig. 13) is developed as 
design implementation to conduct validation study on the task model and collaboration 
planning. From the task model, low level control plan is developed to program the robot 
system and to generate information support to instruct the human operator during 
operation. From the implementation operation, the cable harness assembly operation was 
successfully being completed by the human-robot system based on the proposed 
collaboration planning. A second set of operation with the assembly sequence changed in 
switching Task 3 and Task 4 (Section 4.1) was also successfully being conducted to validate 
the assembly sequence planning in task modeling. 

Operation performance evaluation was also carried out based on the comparison results 
between conventional manual assembly setup (Exp I) and the new human-robot 
collaboration setup (Exp II) in Fig. 13. A group of novice and expert operators (7 males, 22- 
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36 years old) had performed the assembly three times each in each of the two setups to 
obtain the time needed to complete the operations. 




Fig. 13. Prototype production cell setup 

From the results in Fig. 14, the overall performance was improved with shorter assembly 
duration in collaboration setup (Exp II). In the collaboration setup (Exp II), novice and 
expert operators had almost the same assembly duration, which meant best performance 
was made possible even for novice operators, who in conventional setup (Exp I) require 
almost double the time in first trial. The improvement in assembly quality, from 10% to 20% 
of assembly error (insertion error) in conventional setup (Exp I) to assembly error was 
totally being prevented in collaboration setup (Exp II), had proven the effectiveness of the 
collaboration. 
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Fig. 14. Results of assembly duration in conventional setup (Exp I) and collaboration setup 
(Exp II) 
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6. Conclusions and Future Work 

The challenge of this work is to develop collaboration planning between human operator 
and robot system in a collaborative manufacturing system by task analysis approach. The 
development is worked in parallel with a cable harness assembly operation in a prototype 
cellular manufacturing system. The core developments of this study are summarized as the 
following: 

(a) Task decomposition by hierarchical task analysis - by using the capability of HTA, 
the entire operation is being decomposed into structured hierarchical tasks tree. 

(b) Collaboration analysis - qualitative and quantitative analyses are conducted to 
identify and justify the possible collaborative solutions from the task model to further 
define the details of collaboration. Collaboration roles are assigned to all task 
components with color task role indicators to improve the collaboration relationship 
representation of the task model. 

(c) Design enhancements in operation process design - improvements in term of (a) 
group repetitive steps, (b) add interval step, (c) preserve collaboration, and (d) 
assembly sequence changes are achieved in the task modeling of cable harness 
assembly. 

(d) Design extensions in human skill analysis, safety assessment and operation support 
- extensions in facilitate human cognitive and motor skills studies, conduct risk 
assessment for safety design and assist information support development. 

(e) Practical implementation in prototype system - model validation was conducted 
successfully with an actual cable harness assembly operation and positive results were 
obtained in the operation performance evaluation. 

This work might have completed a preliminary modeling framework for human-robot 
collaboration planning in manufacturing systems based on task analysis approach. More 
research studies and developments are needed to further enhance the work: 

(a) Quantitative studies should be conducted to investigate the effectiveness of the 
human-robot collaboration planning. 

(b) Comparison study with other production operations to investigate the modeling 
capability of the proposed framework. 

(c) The temporal aspects in collaboration should be taken into consideration to develop a 
more realistic representation for asynchronous human-robot operations. 
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1. Introduction 

Collaboration among robots and human beings has inspired researchers and novelists since 
a long time ago. Apropos, the word "robot" first appeared in a theatre play ("R.U.R.", Karel 
Capek, 1921) referring to an automata character, a slave humanoid. Important advances for 
control strategies were presented by researchers, applied to service robots, toys and 
automata vehicles, concerning the interaction with human beings. 

Over time, manipulator robots were massively used on industrial plants, performing 
predefined and repetitive tasks. Modern applications for manipulators, involving two or 
more robots on cooperative tasks, are now arising in industry. Most of the scientific 
publications on this area present solutions for some aspects involving humans, mainly 
related to the safety in robots' workspaces, and the flexibility to fast operate and reconfigure 
them. However, the way to operate manipulators remains rigidly based on imperative 
programming, through a HRI (Human-Robot Interface). On the other hand, a new approach 
proposed by (Brooks, 1986), based on behaviours, allows the definition of reactive models of 
control applied to mobile robots. The main limitation of this approach is its strictly reactive 
behaviour, i.e. all knowledge the robot will learn about the environment is unpredictable. 
Current trends in several research areas are pointing to a possible occurrence of a new 
singularity, when the mankind will experiment the knowledge disembodiment, i.e. the 
human knowledge will be retrieved from brain, including its consciousness, and transferred 
to another place, or machine (Vinge, 2008). Psychologists (Pinker, 1999) defend that the 
mental states, as well as deliberations and emotions, can be represented by means of 
symbols of a mental language, known as "Mentalese". The free representation of signals and 
symbols for all mental states and their causalities is practically impossible, considering the 
current state of the art in technology. However, if conceived to specific domains, this can be 
fetched. Rules and policies for collaborative environments consist of well formed sentences, 
which describe states, causal relationships and their effects, applied to collaboration among 
human beings. These rules and policies have been used for several situations, involving 
computer supported cooperative work (CSCW). 

This chapter presents and discusses the application of symbolic rules to coordinate 
collaborative environments with manipulators and humans. It also demonstrates how to 
express a set of collaborative rules, with common effects for machines and humans. We 
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know that the elephants don't play chess (Brooks, 1990), but at the end of this chapter was 
presented our "robotic elephant" which plays a Tic Tac Toe game. 

2. Robot control strategies 

The traditional approach for robots control is feedback based and hierarchically 
subordinated to a trajectory generator that is responsible for mapping tasks into sets of 
movement sequences (rotations and translations). These sequences are obtained by 
combining the individual motions defined by links or mobile parts of a robot. The generated 
information is presented to the control modules, which are responsible for motors actuation. 
A different strategy was presented by (Brooks, 1986). It describes a multilayer architecture 
with several levels of abstraction, allowing reactive behaviours (Nwana, 1996) for mobile 
robots. Each layer consists of a distinct level of competence, which may be activated. Thus, a 
layer can modify the resultant output, by including its component on the signal, and also 
inhibiting the signal produced by the lower layers. 

2.1 Manipulators and the traditional approach 

The large majority of industrial robot manipulators, available in the market today, use the 
control architecture originally proposed by Engelberger/Devol and their UNIMATE robot 
more than 30 years ago. The movements are decomposed by the user during the 
programming task into a sequence of primitive movements, i.e. point to point, straight line, 
circle. Normally the robot controller programming interfaces are implemented in an 
interpretative form. All the movements are related to a "tool center point - TCP". Time 
intervals or velocity to be reached during the execution of each primitive movement is also 
user defined using the programming interface. 

A strong point of this strategy is the capability to generate complex behaviour and 
movements independently on which kind of end-effector the manipulator is carrying on. A 
weakness of this approach remains in the ability of such equipment to interact with a non- 
static environment, as for example, in assembly tasks. Each primitive of an individual 
movement is further transformed into coordinates for the joints using inverse kinematic 
calculation. The coordinates and its derivatives are finally transmitted to the robot 
controllers, for each single joint, as a function of time. 

2.2 Cooperative robots, humanoids and the behavioural-based approach 

Cooperative applications research for multiple robots sprung in the last decade (Parker, 
2003). (Cao et al, 1997) state that cooperative behaviour can be observed on more complex 
animals (vertebrates, for instance), including human beings. Such behaviour has social 
motivation, demanding each isolated participant to feel the need or desire to cooperate. A 
system with multiple robots can present cooperative behaviour if, when performing some 
task with any cooperative mechanism, the increase on the efficiency of the whole system 
emerge. 

Several architectures were proposed to solve the distributed control problem for mobile 
multi-robots, but this subject is out of the main focus of this chapter. A good description 
about this research area can be found in (Parker, 2003). On the other hand, there are few 
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published works (Lau & Ng, 2006) that discuss solutions for control problems for robotic 
manipulators using distributed strategies. 

Reactive behaviour models 

Behaviour may be defined (Bishop & Potter 2004) as an observable and repeated pattern in 
the relationships among spatio-temporal events associated with an agent and its 
environment. Behaviour based robots use the information they gather from the environment 
through the sensors to react to specific situations. The internal representations of 
environment are extremely limited when not completely inexistent. Isolated models of 
simple behaviours are responsible to respond to specific sensor signal conditions. The 
overall robot behaviour results from the output combination of each model. It is exactly the 
synthesis of coherent reaction, i.e. emerging intelligence as the result of the fusion of each 
behaviour model constitutes an open challenging task. 

Reactive models are very important for strategies involving robots learning, especially 
mobiles ones, because they allow to assume the world as its own best model (Brooks, 1990). 
This feature is primordial in situations involving adaptation for robots' behaviours acting in 
unknown environments, like other planets. 

Deliberative behaviour models 

In deliberative control, the robot takes all of the available sensory information, and 
compares this information with its internally stored knowledge. Therefore in this approach 
a complete representation of the environment is stimulated using all available internal robot 
computer resources. To accomplish its task, the robot must further plan its future actions. 
This requires the robot program to look ahead. As a consequence, the control structure must 
provide multi task real time capabilities allowing the robot to act strategically. 
If we pretend to see robots replicating behaviours based on the knowledge previously 
obtained by a human (in opposite to a non-deterministic learning by observing the 
environment), this knowledge must be formally expressed and converted into deliberative 
actions, according to this interpretation. Currently, two different approaches may apply: the 
neuronal model of the brain and the symbolic model of the mind. 

The first (connectionist) aims to reproduce in computers the basic functions of the brain 
inspired by its topology. The main limitations for this approach refer to the enormous 
quantity of neurons and synaptic connections, and also the plasticity of neural networks 
created by the brain. 
The symbolic model is presented and discussed at session 3.1 of this chapter. 

2.3 Distributed and modular strategies for control architectures 

Modular Robotics offers an answer for various complex tasks. Instead of designing a new 
and different mechanical robot for each task, simple module reconfiguration when connect 
in a suitable form may accomplish complicated things and meet the demands of different 
tasks or different working environments. 

Each module is improved with individual capabilities for processing, sensing, 
communicating and actuating. The overall functionality of a modular system is only 
achieved when several modules are connected as a unique robot, i.e. a single module 
presents low utility. 
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Similarly, a manipulator robot can be decomposed and its parts individually analyzed. 
These parts present individual capabilities, like modules. Thus, the robot can be classified as 
an n-modular system, where n is the number of different types of modules. 

2.4 Supporting collaborative behaviours 

A multilayer control model, adapted from (Brooks, 1986), was proposed in (Martins Jr et al, 
2008). This new approach includes cooperative and collaborative behaviours, and was 
designed to operate on distributed systems, defining different contexts - local and global - 
as shown in Figure 1. 
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Cooperative Behavior 




Virtual Environment Creator 
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Trajectory Generator 












Task Scheduler 




Motor Controller 









Fig. 1. Multilayer control model 



Distinct parameters of criticality and strictness for agents operating on each of the two 
contexts (local and global) can be individually treated into its respective layer and provide 
means to define its coupling degree to the target (robot). Figure 2 shows the appropriated 
allocation of the layers on a distributed environment. 

The local functions describe processes that are highly rigorous for execution time, but 
demand a small amount of resources (storage and processing power). They can be classified 
as local agents, tightly coupled to the target. 

On the other hand, in the global context, the processes are less rigorous with respect to 
performance time, but need larger amount of resources. These features indicate that the 
designed agents must not be embedded into the target, but executed on loosely coupled 
remote computers. Local agents interact directly with the target using communication 
boards connected to actuators and sensors. 

Each distinct part (link) of the robot, including its sensors, motors and mechanisms can be 
individually considered as different modules. Thus, the movements' composition for each 
module can be resolved on a higher level, as a cooperative task. This is one of the 
advantages provided by the architecture. 
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Remote agents can be assigned for each module and interact with local agents to provide 

appropriate behaviour for the global model of the environment. This interaction can be done 

through a local network, and supported by data communication protocols (such as CORBA, 

or HTTP). The cooperative behaviour can be achieved by joint deliberation involving remote 

agents, based on cooperation rules and on environment model. Cooperation is frequently 

associated to a specific task execution. 

The collaborative behaviour is placed on top of the architecture. Collaboration can be 

observed when a robot interacts with a set of tasks contributing to achieve a common goal to 

other agents in the environment, including humans. In the same manner, this behaviour is 

deliberated from decisions taken by agents, by analyzing collaborative rules and checking 

the current state of the environment. 

The guiding rules for cooperation and collaboration are stored in a rules database, and can 

be accessed by global agents, at its respective actuation level. 

A brief description of each architecture layer is presented on the following subsections. 

Motor controller 

Motor controller describes the bottommost layer, highly dependent and coupled to the 

hardware. Software agents developed to this level are locally executed (embedded) on the 

target. Real time requirements are highly rigorous, requiring performance time up to 1 

millisecond. 

The main function of this layer is to emit signals to the drivers that directly actuate on each 

of the motors. Data from sensors (encoders) attached to the motors are returned to the 

control mesh as feedback, to ensure correct performance of the actuators. 
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Task scheduler 

Task scheduler plays an important role in the local context of the architecture, because it 
provides means to coordinate the processes that operate during each of the individual 
movement of the robot links. The execution of individual movements is part of the strategy 
defined by an upper layer (trajectory generator), allowing general repositioning of the robot 
in the environment. 

Trajectory generator 

Trajectory generator is the topmost layer in local context. It also imposes rigorous 
restrictions concerning real time, yet it allows larger deadlines, of around 10 milliseconds. 
Its main function is to define individual movements for each motor, by decomposing a 
desired trajectory between two points and sending it to the robot (inverse kinematics). 

Virtual environment creator 

The bottommost layer of global context is to implement the general model of the 
environment where the robot will be placed. At this level, geometric aspects are considered, 
allowing robot's workspace analysis to be performed. Visual and proximity sensors can 
provide data to be compared with the current state of the model, internally represented by a 
software agent. Global strategies to replace the robot in the environment can be defined at 
this level. These functions demand more computing power and, as compensation, allow 
flexibility to response times. The main advantage of an environment model which keeps 
close fidelity to the real world is the easiness to perform robot simulation on a virtual 
environment. In this manner, all the development and testing stages for high-level 
functionality, involving cooperative and collaborative behaviours, can be previously done 
on a simulated environment. 

Cooperative behaviour 

Cooperative behaviour describes relations among robots (or parts of them) and must be 
implemented by global agents, based on environment model and predefined rules (rules 
database). It is possible to notice behavioural capacities, both reactive and deliberative, of 
these agents. The reactive capacity is provided by environment analysis, which represents 
its internal model, differently from the deliberative, which results from decisions about 
cooperation rules. 

Collaborative behaviour 

In the same manner, collaborative behaviour for human interaction can be also provided. 
Previously defined collaborative rules are stored in a database and allow analysis by agents, 
by comparing to the current state of the environment model. 

Rules database 

The rules database is an important artifact of the whole architecture, and it was designed to 
store all cooperative and collaborative rules. These rules were defined using the M-Forum 
model (Camolesi Jr & Martins, 2006), and describe interaction policies by means of 
collaborative situations, involving different agents. M-Forum is presented in the next 
sessions. 
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3. Languages and rules 

Languages define written or spoken symbols that are used for communication purposes. 
Written symbols can be jointly combined into words that, and depending on the context, 
provide the meaning of transmitted ideas. The sentences composition in a language is 
previously defined by a grammar. Sentences are constituted by a finite sequence of symbols 
from some finite alphabet (Slonneger & Kurtz, 1995). 

The syntax of a programming language describes how the symbols may be combined to 
create well-formed sentences (or programs). The meaning, obtained by interpreting the 
words of a sentence, defines its semantics (originally conceived in "Mentalese", the 
language of thought). Thus, intentions about a desired behaviour can be described by means 
of rules, using an interpretable language. 

Restricted and non-ambiguous formal grammars were proposed to express programming 
languages for computers. BNF (Backus-Naur form) is a widely adopted formal model to 
specify grammars that describe terminal symbols of a valid alphabet, non-terminal symbols 
and production rules. The main benefit of using a BNF style language is the easy to 
implement programs that work as its lexical interpreters. 

3.1 Symbolism and the mind-brain dilemma 

Symbolism can be described as a movement that defends symbolic representation of mental 
activities, inspired by computer like way of operation. In this sense, a constructive approach 
is defined using a top-down strategy (Minsky, 1990): begin at the level of commonsense 
psychology and try to imagine processes that could simulate it. The central idea consists, 
assuming a greater challenge, to search for a solution by decomposing it into simpler parts. 
This refers to a reductionist method, a typical approach commonly applied in AI (Artificial 
Intelligence), and known as heuristic programming. 

Two relevant aspects can be highlighted from the concept of the mind presented in (Pinker, 
1999), which constitutes the foundation of computational theory of the mind. The first states 
that the mental computations are applied to information, and this can be expressed using an 
internal symbolic representation. The other refers to the functional composition of the 
mental modules, which perform the computations. Thus, no matter what kind of subject 
(physical) where mental computation is performed, the functionality of the modules that 
compose the mind and the symbols are submitted to it. As a consequence, beliefs and 
desires can be seen as information, physically embodied as configurations and symbols. 
In the last decades, with the advances in AI research, a new approach for philosophy of the 
mind - not dualist either materialist - has emerged, the functionalism. Functionalism 
introduces the concept of causal role, in which a mental state can be described by their 
causal relations with other mental states. Functionalism is based on the distinction 
established by computer science about hardware (physical components) and software 
(programs). From this point of view of psychology, a system can describe a human being or 
a machine, and its basic constitution (neurons or electronics) is not what really matters, but 
how parts are organized (Fodor, 1981). Thus, functionalism does not rule out the possibility 
of a mechanical or electronic system having mental states and processes. 
The central subject of this chapter is related to collaboration among robots (machines) and 
human beings. In this sense, we have adopted a top-down model, where the behavioural 
rules, with common sense for both types of actors, have been stated using a formal language 
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(constituted by symbols). Then, the rules were implemented and their functionality 
observed during the coordination of a collaborative activity (more specifically, a game). 

3.2 Behavioural rules, policies and collaborative environments 

Interaction Policies are norms for the interactions in an environment; those can be 
established by logic grouping of rules with well defined goals or objectives. In the definition 
of a Collaboration and Control Policy (CCP) model for human-robot interaction, a policy 
must observe the relationship among robotic and human agents in a same environment, 
regarding collaborative task performance. 

The research (Camolesi Jr & Martins, 2005) has achieved excellent progress for structure and 
ontology definition. However it still has a lot to advance on applications, such as robots 
control. Towards the approach of these questions, the M-Forum model supports 
collaborative interactions modelling through the definition of rules by providing support to 
five dimensions: actor; activity; object; time and space. A comparison between M-Forum and 
the other models for rules (Tonti et al, 2003) is presented in Table 1. 





Kaos 


Rei 


Ponder 


M-Forum 


Ontology 
based 


Yes 


Yes 


No 


Yes 


Specification 
language 


DAML/OWL 


Prolog based 


Ponder 
Language 


L-Forum 


Tool for 
specification 
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KPAT - Graphical 
Editor 


No (GUI under 
development) 


Graphical Editor 


No (GUI under 
development) 
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Java Theorem 
Prover 


Prolog engine; 
Event-condition- 
action model. 


Event calculus 


Activity theorem; 
Deontic theorem; 
Event-condition- 
action model. 


Enforcement 
mechanisms 


Policy automation 
being explored for 
the next version 


Action execution 
is outside the Rei 
engine 


Java interfaces 
for enforcement 
agents 


Rule execution is 
outside the engine 


Flexibility 


Ontology can be 
extended with 
domain dependent 
descriptions of 
local entities 


Ontology can be 
extended with 
domain 
dependent 
descriptions of 
local entities 


Management 
domain as a 
structuring 
technique for 
partitioning 
complex object 


Ontology can be 
extended with 
domain dependent 
descriptions of local 
entities 


Elements 
represented 


Actors, actions, 
groups, places 


Subject, activity, 
object 


Subject, activity, 
object, domain 


Actor, activity, object, 
time, space, 
association, domain, 
composition and 
generalization 
abstractions 



Table 1. Comparison between M-Forum and other models for rules 



3.3 The M-Forum model 

In M-Forum (Camolesi Jr & Martins, 2006), the Actor dimension allows the representation of 
an agent in a collaborative environment through activity rights, prohibitions and 
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obligations. The actors of a collaborative environment can be classified in human or not- 
human. Every human actor has an identifier (Ach_id), a current state (AchState) and a set of 
attributes (Ach_AttS). Given qh as the number of human actors at the environment and qs, 
the number of not-humans, the formal statements are: 



AchS = {Achi,Ach2,...,Achqh} , AcsS = {Acsi,Acs2, 


...AcSqs} 


AchS * v AcsS * 




AchS n AcsS = 




AcSS= AchS u AcsS 




Achi=(Ach idi,AchStatei,Ach Att&) 




Acsi = (Acs_idi , AcsStatd , Acs_AttSi ) 





Actors are responsible for the execution of individual or collective activities, thus being able 
to reach objects, an actor or actors group. 

Activity is an execution unit that can be carried through by an actor or group. Normally, 
activities involve the manipulation or transformation of an object. Activities must be defined 
using Activity Operators and logic Operators representing rights, prohibitions and 
obligations. Activity Operators are required to specify the interaction between actors and 
objects. Activities have identifiers (At_id), a state (AtState), an activities subset (At_S), a set of 
operations (OpS) and a set of attributes (At_AttributeS). Given qa as the number of activities 
in an environment: 



AtSS 


= {Ati 


At!, 


...At 


jaj 






At= 


(At idu 


AtStatei 


AtS, 


OpS, 


, At Attribute Si) 


AtSi 


= AtSS 












OpS, 


cOpS 













Object is any element that can be used in actions on objects or actors. An object represents 
the structural characteristics and the behaviour of reality. Activities can be carried through 
in objects to modify its characteristics. An object modelling in such a way establishes 
uniformity of vision and treatment, useful for collaborative environment projects. An object 
may be composed by others objects (CompObS) and has an identifier (Ob_id), a state 
(ObState) and a set of attributes (Ob_AttS). Activities and Operations may be performed on 
Objects that allows its state or attributes changing. 



ObSS = {Obi, Obi, ..., Obq»}v ObSS *<Z, 
0b,= (Ob_idi, CompObS,, ObState,, Ob_AttS,) 
CompObS, c ObSS 



Spaces are localization areas of actors or objects and the specific areas used for activities. 
Like other elements presented in this section, the spaces are essential for modelling a 
collaborative environment. 

On the collaborative interaction, elements of the dimension space must be defined using the 
Space Operator (SpOp) to specify the position or the size of actors and objects in 
collaborative environments. The space element has an identifier (Sp_id), a state (SpState) and 
a set of attributes (SpAttS). If qe is the number of spaces into an environment: 
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Spi = (Spidi , SpStatei, SpAttSi) 

SpSS = {Spi,Sp2,..., Sp q e} 

SpOp e So = { <,<=>,>=,=,<> ==(attribution), not equal, 

inside, outside, intersect, meet, overlap, north, south, east, west } 



In time modelling, Duration, Date and Occurrence have basic semantic for temporal 
references establishment. These semantics are used to define a logical action with duration, 
occurrence date or occurrence interval of activities defined on interactions between actors 
and objects. 

The formal basis for temporal elements describes the natural set of numbers (N), and 
representations for years (Ty), months (Tm), days (Td), hours (Th), minutes (Tmi) and 
seconds (Ts), for a Moment or Interval. Enumerated sets of relative values (Tmr, Tdr, Thr, 
Trnir, Tsr) are used to represent dates for a specific calendar. Given qt as the number of time 
moments or intervals occurring on an environment: 



Ty, Tm, Td, Th, Tmi, Ts e N 

Tmre {1,2,3,4,..., 12} 

Tdr e {1,2, 3, 4, 5, ...,31} 

Thre {0, 1,2, 3, ...,23} 

Tmire {0, 1,2, 3, ...,59} 

Tsre {0, 1,2, 3, ...,59} 

Time = {Tiid ,(Ty , Tm , Td , Th , Tmi , Ts)} 

Datetime = {Tiid ,(Ty , Tmr , Tdr , Thr , Tmir , Tsr)} 

Tb, Te e Datetime , Interval = {Tiid , (Tb, Te)} 

TiSS = Time u Datetimeu Interval 

TiSS = {Tii,Ti2,...,Ti q t} 

TiOp e To = { <<=>,>=,= <>== (attribution), 

precedes, succeeds, directly precedes, directly succeeds, 

overlaps, is overlapped by, occurs during, contains, starts, 

is stated with, finishes, is finished with, coincides with } 



The dimensional elements of a rule are defined in three contexts: 

- Applicability: condition for the execution or activation of a rule and definition of 
the scenes (values of attributes or space aspects) in which it can be applied; 

- Execution: a set of expressions that establishes the actions or conditions for the 
interactions between elements, being able to optionally involve transitory aspects 
of time and space; 

- Survivability: it is an optional context specifying the other rules with the same 
applicability. Also the scenes can be defined (values of attributes or space aspects) 
to establish the instant at which the rule must be activated or deactivated. 



3.4 The L-Forum syntax 

L-Forum is a language developed to formalize the concepts specified by the M-Forum 
model. The language allows the definition of rules for an environment, increasing their 
precision and improving disambiguation for collaborative environment designers. Its 
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overall structure may be described by clauses, which are defined for three particular 
purposes: 

- Context: this clause is composed by performing or activating parameters of a rule 
and comply with applicability conditions of the scenario (value of attributes, spatial 
or temporal aspects) for a rule adoption; 

- Definition (body): it is composed by a set of expressions where actions or 
conditions are established for interacting elements and may involve transitory 
aspects of time and space. Rules and actions may be directly invocated at the body 
of a rule, which allows to compose the expressions; 

- Regime: this is the scope of a rule, and refers to an optional set composed by 
interrelated rules having the same orientation to be performed or applied. 
Scenarios, involving a rule activation or deactivation, can be also described. 

The main elements of L-Forum syntax are presented in Table 2. 

3.5 Collaborative rules for human-robot interactions 

At this point, we address to the problem of defining the collaborative rules among robots 
and humans. To illustrate it, a simple task was considered: a tool passing between the 
human and the robot. 

For the robot to identify the different collaborative situations, involving the task, a visual 
code was established. If the human presents his(er) open hand over the common workspace 
(a table surface), the situation "tool passes from robot to the human" must be assumed. If 
the human presents his(er) hand holding the tool, the opposite situation must be considered. 
Summarizing, the dimensional elements to elaborate collaborative rules, are: 

- Actors: robot and human. The robot is an actor composed by different links. The 
human being is also an actor established by the composition of single parts, 
detaching the hand; 

- Objects: the tool, which will be collaboratively shared by the actors; 

- Space: there are three involved spaces in the problem: the common workspace 
(table surface) where will be shared to pass the tool; the individual spaces, where 
the human and the robot stay. Each individual space is exclusive. Only the 
common area must be shared collaboratively by both actors; 

- Activity: ordering and delivering are activities that may be realized by both actors 
(human and robot). The activities will be recognized by both actors analyzing the 
state their parts, i.e., the human's hand and the robot's gripper (end-effector). A 
hand or a gripper on "open" state means the tool ordering; a hand or a gripper 
holding the tool is associated to a tool delivering. Grasping and releasing are also 
related activities on the working process. 

When modelling the collaborative actions, the human is the actor with primary actuation, 
and so, he (or she) establishes the frequency and sequence for actions. In this sense, 
considering the dimensional elements of the collaborative work scenery, previously 
presented, some rules to compose the Collaboration and Control Policy (CCP) are shown in 
Table 3. 
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<rule> :: = 


'Rule' <rule name> '['<status>']' '{' <context> 




'Body ::'<definition> [<regime>] '}' 


<context> ::= 


'Parameters: ('<parameters>')' [ <applicability> ] 


<definition> :: = 


<condition> | <action> | <rule call> [<definition>] 


<regime> ::= 


<survivability> ['Priorities:' <priority>] 


<parameters> :: = 


''any' | 'all' | <identifier>)':'<element> [',' 




<parameters>] 


<element> :: = 


<actor> | <group> | <object> | <space> | <time> | 




<associatk>n> | <activity> | <operation> 


<type> :: = 


'actor' | 'group' | 'object' | 'space' | time' | 




'association' | 'activity' | 'operation' 


<applicability> :: = 


'Applicability:: ' <condition expression> 


<survivability> :: = 


'Survivability::' -(condition expression> 


<condition> :: = 


'If ' <conditk>n expression > then{' <definition> '}' 




|'else{' <definition>'}'] 


<action> ::= 


'Action: (' <actor> <normative operator> {<activity> 




'<actor> | <object>)} [<space attribution operation> 




<space>] [<time attribution operation> <time>] )[');' 




<action>] ');' 


<supreme action > :: = 


<actor> <normative operator> <primitive opera tor> 




< element >| <domain>| ('is part of | is a) 




<element>) | 




<actor> <normative operator> <primitive group 




operator> <group> <element>) 


<definition action> :: = 


<actor> 'set' <status> 


ottribution action> :: = 


<actor> 'attribute' (<value>| <formula> | ( (next | 




prior) (<valuedomain>| <domain name>) ) <attribute> 


<condition expression> ::= 


'(' (<attributexattribute condition 




operator>(<value>| (['all' | 'any'] (<value 




domain> | < domain name> ) | (<condition expression> 




and | or ) ')' 


<rule call> :: = 


'Rule (' <rule name> ' ('<parameters> ')' <normative 




operator> [');' <rulecall>] ');' 


<priority> :: = 


<name> [',' <priority>] 


<group> :: = 


<name> :Group 


<actor> :: = 


<name>':Actor' 


<activity> ::= 


<name> ':Activity' 


<operation> :: = 


<activity> . <name> ': Operation' 


<object> ::= 


<name>':Object' 


<space> :: = 


<name>':Space' 


<time> ::= 


<name>':Time' 


association > ::= 


< elem ent > ' . ' < name > [ ' . '< association >] ' Association ' 


<attribute> :: = 


<element>'.'<name> [':Attribute'] 


<domain> :: = 


<name> ( <valuedomain> | <grouping> ) 


<value domain> :: = 


'(' (<numeric value> {',' <numeric value>}) | 




;<string> <',' <string>}) ')' 


<grouping> ::= 


<type> <name> <attribute condition opera tor> ( 




<value> | (['all' | any ](<value domain >| <domain 




name>))) 




[(and | or) <grouping>} ) | 




(<element> {',' <element>}) 


<element status> :: = 


<element>< status attribution operator> <value> 


<status> :: = 


'active'] | ['inactive'] 


<primitive group operator> ::= 


'insert' | 'delete' | 'update' 


<primitive operator> ::= 


create' | 'destroy' 


<group element opera tor> ::= 


'€' I 'e' I 


<group group operator> :: = 


'e' I V I 'c' 


octivity condition opera tor> :: = 


/not'] 'has' 


<normative operator> :: = 


'right' | 'prohibition' | 'obligation' | 'dispensation' 


octivity attribution operator> ::= 


'receive' 


<status attribution opera tor> :: = 


'put on' | 'move to' 


<space attribution operator> :: = 


'==' | 'inside' | 'outside' | 'north' | 'south' | 'east' 




| 'west' 


<time attribution operator> ::= 


'in' I 'on' I 'at' 



Table 2. L-Forum syntax 
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Rule Human Delivers Tool [active] { 


Parameters:: 


(hu: human, ro: robot, too: tool, ts: table Surface) 


Applicability:: 


(hu.hand not is open) and (ro.gripper is open) and 




(hu.hand not is on ts) and (ro.gripper not is on ts) 


Body:: 


Action (hu obligate hand put on ts); 




Action (hu obligate release too on ts); 




Action (hu.hand obligate put of ts); 




Rule Robot Moves to the work (ro , ts) 




Action (ro obligate hold too); 




Action (ro.gripper obligate put of ts); ( 


Rule Human Orders Tool [active] { 


Parameters:: 


(hu: human, ro: robot, too: tool, ts: table Surface) 


Applicability:: 


(hu.hand is open) and (ro.gripper not is open) and 




(hu.hand not is on ts) and (ro.gripper not is on ts) 


Body:: 


Action (hu.hand put on ts) 




Action (hu.hand obligate put of ts) 




Rule Robot Moves to the work (ro , ts) 




Action (ro obligate release too on ts); 




Action (ro.gripper obligate put of ts); 




Action (hu.hand obligate put on ts) 




Action (hu obligate hold too); 




Action (hu.hand obligate put of ts) } 


Rule Robot Moves to the work [active] { 


Parameters:: 


(ro: robot, ts: table Surface) 


Applicability:: 


(hu.hand not is on ts) 


Body:: 


Action (ro.vector_a prohibition move on ts) 




Action (ro.vector_b prohibition move on ts) 




Rule Moving Vector_c (ro, ts) 


Survivability:: 


Priorities: Human Delivers Tool , Human Orders Tool ] 


Rule Moving 


Vector_C [active] { 


Parameters:: 


(hu:human; ro: robot, ts: table Surface) 


Applicability:: 


(hu.hand not is on ts) 


Body:: 


Action (ro.vector_c obligate move to ts) ] 



Table 3. Rules for collaborative tool passing 



4. Case study: Tic Tac Toe game 

In this session we present an experimental case study, involving human and robot 
interaction faced as opponents in a board game, known as Tic Tac Toe. The game was 
chosen because its rules are very easy to learn, allowing it to be played by people with 
different levels of skill, from children to adult. 
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The decision was also influenced by another feature, that the game is played on a predefined 
board (field), facilitating the coordination of movements done by opponents (robot and 
human) into the common workspace. 

This case study was proposed as a proof of concept for using collaborative rules to govern 
the interactions among different types of actors, a robot and a human. It was also important 
to demonstrate the need for a strategy definition when selecting rules in collaborative 
environments, in order to surpass the unpredictability of some human behaviour. 

4.1 The game 

The Tic Tac Toe is a two player game where the participants take turns drawing tokens (X or 
O) on a 3 x 3 grid. Winning the game involves a player placing three tokens in a row, 
column or diagonal. When the grid is completely full and no sequence of three equal tokens 
occur (row, column or diagonal), they got a draw. 

Figure 3 shows a particular and possible situation during a Tic Tac Toe match. In this case, 
the player of X tokens won. 



o 



X 



o 



o 



X 



X 



X 



Fig. 3. A Tic Tac Toe situation 

An expert performance for Tic Tac Toe game can be described as a set of few rules (Crowley 
& Siegler, 1993), as shown in Table 4. These rules are enough to describe every faced 
situation, during a Tic Tac Toe game, but often occurs that more than one rule can be 
applied, pointing to the need to define a criteria to select the most appropriated. 



Adaptability for different levels of skill 

Despite the rules simplicity, selecting them in order to make a move depends on several 
factors, like attention, knowledge and others. These factors are clearly influenced by the 
player's age, and thus, the system must be able to use appropriate strategies for different 
levels of skill presented by its opponent. Otherwise, a child will never be able to win, and 
could become bored. 
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Move type 


Conditions 


Action 


Win 


If there is a row, column or diagonal with two of my 
pieces and a blank space 


Play the blank 
space 


Block 


If there is a row, column or diagonal with two of my 
opponent's pieces and a blank space 


Play the blank 
space 


Fork 


If there are two intersecting rows, columns or diagonals 

with one of my pieces and two blanks 

AND 

If the intersecting space is empty 


Move to the 

intersecting 

space 


Block fork 
(1) 


If there are two intersecting rows, columns or diagonals 

with one of my opponent's pieces and two blanks 

AND 

If the intersecting space is empty 

AND 

If there is an empty location that creates a two-in-a-row 

for me 


Move to the 
location 


Block fork 

(2) 


If there are two intersecting rows, columns or diagonals 
with one of my opponent's pieces and two blanks 
AND 

If the intersecting space is empty 
AND 

If there is NOT an empty location that creates a two-in-a- 
row for me 


Move to the 

intersecting 

space 


Play center 


If the center is blank 


Play the center 


Play 

opposite 

corner 


If my opponent is in a corner 

AND 

If the opposite corner is empty 


Play the 

opposite 

corner 


Play empty 
corner 


If there is an empty corner 


Move to an 
empty corner 


Play empty 
side 


If there is an empty side 


Move to an 
empty side 



Table 4. Set of rules for expert performance on a Tic Tac Toe game 



4.2 Defining the rules of the game 

The next step consists on translating rules to L-Forum format. According to L-Forum syntax, 
described above, a rule may be stated using some elements of the language. A rule must 
have a unique name and declare its status. The parameters of a rule specify the involved 
elements, like actors, space and objects. The applicability refers to the conditions that cause a 
rule selectable. The body of a rule describes actions to be performed. 

Two examples for rules mapping are presented in Table 5. The first refers to the "Play 
Center" rule for a Tic Tac Toe expert match, and may be applied when the center is empty 
and the game is not finished. The second implements a rule that may be selected in four 
situations, relating to each corner of the board. 
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Rule PlayCenter[active] { 

Parameters:: (pi :player:actor; tboard :TicTacToeboard : space; tok :token:object, 

:game:object) 
Applicability:: (tboard. center is empty) and (g not is finished) 

Body:: Action (pi right play tok inside tboard. center); } 



Rule PlayCorner[active] { 

Parameters:: (pi :player:actor; tboard :TicTacToeboard : space; tok :token:object) 

Applicability:: (g not is finished) 

Body:: if (tboard. corner_high_right is empty) 

then { Action (pi right play tok inside tboard. corner_high_right ); 

} 

else { 
if (tboard.corner_high_left is empty) 
then { Action (pi right play tok inside tboard. corner_high_left); 

} 

else { 
if (tboard. corner_low_right is empty) 
then { Action (pi right play tok inside tboard. corner_low_right); 

} 

else { 
Action (pi right play tok inside tboard. corner_low_left); 



Table 5. "Play center" and "play corner" rules translated to L-Forum 

4.3 The hardware infrastructure 

An IBM 7545 SCARA robot retrofitted with open platform (Aroca et al, 2007), was used in 
this project. The target's hardware, assembled on a CompactPCI rack, contains the following 
components: 

- Boards: Inova AMD K6, Acromag Carriers, National Instruments I/O; 

- Industry Packs (IP): Tews 48 Digital I/O, Tews IP Quadrature, Tews DAC. 
Figure 4 presents the whole view of the architecture. 

Since the main purpose of this project was not related to research accurate positioning and 
fine control, and aiming to simplify the robot operation, a strategy based on fixed points was 
adopted. All the nineteen positions, representing valid locations of the game, were 
predefined and marked into an 800x400 mm board, as shown in Figure 5. Ten of these 
positions (five at each side of the game field) were designed as pieces repositories. 
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Fig. 5. Design of the Tic Tac Toe board 



Nineteen reed switches (magnetic presence sensors) were fixed on the board, at each 
position for pieces locations (marked with small circles on the board). The reed switches are 
used to detect magnetic field generated by magnets, which in this case, were inserted in each 
piece of the Tic Tac Toe game, as shown in Figure 6. 




Fig. 6. Magnetic pieces 



All sensors were connected to a board, with 32 digital inputs. This board multiplexes all 

digital entries through a parallel interface, connected to a computer port. 

Since the main focus of this project was to present coherent means to allow robot and 

human collaboration, another subject comes up. Several researches in Robotics have pointed 

to the relevance of robot's appearance when interacting with humans. Human beings 

usually feel more comfortable to interact when the other subject looks familiar. 

Considering that this robot must interact with human beings, including children, we decide 

to give it a playful look. The SCARA robot was dressed in an elephant costume, making it 

very fun and with a less formal aspect. 

Figure 7 shows the robot during a Tic Tac Toe match. The picture also presents the board 

and the pieces over it. 
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Fig. 7. Robot dressed as an elephant 



4.4 Software design and implementation 

The local software agents were designed to execute specific roles, implementing the 
bottommost levels of the distributed model, i.e., the trajectory planner, the task manager and 
the motor controller (Aroca et al, 2007). 

The base system was developed using a real time interface to the Linux kernel - RTAI 
(RealTime Application Interface). Some of its features are the actuator for motors, a PID 
(Proportional Integral Derivative) controller, and sensor acquisition, through an industrial 
PC. The infrastructure also allows interacting with the robot, across the local network. 
The overall solution was based on host-target model. A computer (host) was being used to 
develop and compile the software, before it was embedded in the industrial PC (target). 
Both computers run Linux operating system, but only the target's kernel was increased by 
the real time modules and the RTAI interface. Other facilities were also implemented, 
allowing more flexibility in robot reconfiguration and high level protocols for data 
communication (Tavares et al, 2007). 

A three-dimensional HRI (Human-Robot Interface), called Scara3D, was presented in 
(Martins Jr et al, 2008). The interface was developed to perform tests for high-level layers 
integration into the architecture. The obtained results were satisfactory and proved the 
feasible implementation for the "Virtual Environment Creator" layer of the proposed model. 
When designing the game we considered the interaction among a SCARA Robot and human 
beings, which can be classified as actors according M-Forum specifications. The SCARA 
Robot is composed by translational and rotational links and has a pneumatic gripper, while 
a Tic Tac Toe game contains a board and pieces (X and O); these individual parts are 
represented as objects in Forum model. These relationships are presented in a class diagram, 
as shown in Figure 8. 

Current state of the real environment can be monitored by sensors integrated into the 
architecture, and so the virtual model can be updated, representing the interaction among 
several objects and actors within the game. 
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Fig. 8. Class diagram for Tic Tac Toe game 

As described above, sensors were integrated to the system to detect the presence of pieces 
on nineteen different positions of the board. Another presence sensor (currently, a single 
switch) was also included to detect the presence of a human being into the shared 
workspace. The states of these twenty presence sensors are monitored by a client application 
that fires UDP (User Datagram Protocol) messages into the local network. Thus, using an 
UDP server (UDPMessageReceiver), the system allows asynchronous messages reading and 
performing event passing through appropriated listener implementations. 
The current positions for servomotors were obtained by the system using an encoder 
monitor, which submits TCP (Transmission Control Protocol) requisitions to the target that 
controls the robot. TCP messages are also sent to the target by Motor 'Actuator to reposition 
the servomotors, according the current states of their virtual representations by means of 
ServoMotor class instances. 

As mentioned, listeners were used to provide events communication about states changing 
across objects into the virtual environment representation. Every change among virtual and 
real environments is communicated using TCP or UDP messages, allowing the distribution 
of the system components and the integration between high and low-level layers of the 
architecture. 



5. Conclusion 



In this chapter was presented a new architecture for robot control, which provides layers 
including deliberative behaviour on robot operation. The other features of the proposed 



Collaborative rules operating manipulators 153 

model refer to the explicit definition of local and global contexts and its operating support 
for distributed environments. 

The collaboration among robots and human beings was described using a symbolic 
representation, through a formal model of rules. This approach was successfully 
experimented in restricted situations, describing human-robot interactions. An experimental 
case study was also presented for this purpose, involving a collaborative game among a 
manipulator and humans. 

Future research about this subject can be applied evolving the model to support 
representations of other mental states and allowing the extraction of rules from knowledge 
databases. It is also encouraged the use of the model for other situations, including 
collaboration among other subjects (mobile robots or other machines), as uncovered by this 
chapter. 
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Abstract 

This chapter focuses on the dynamic control issues of lightweight robots as well as flexible 
joint robots. The goal is to increase the bandwidth and the accuracy of the trajectory tracking 
control. Besides the joint flexibility, the control design considers the dynamics of the electric 
motor in AC-form i.e. the three phase permanent magnet synchronous motor (PMSM). The 
final system model is a fifth order non-linear system. Based on the theory of integral sliding 
mode control a robust control approach for the trajectory tracking control of rigid-body 
robots is presented at first. This control approach has pole-placement capability despite 
system uncertainties. The controller is then used as the outer position controller for the 
control of flexible joint robots. To handle the joint flexibility, singular perturbation approach 
is employed, resulting in reference currents for the inner current control loop of joint 
motors. For the current control, sliding mode PWM technique is used to overcome the 
disadvantages of conventional open-loop PWM. The developed control algorithms are 
simple enough for practical implementation and verified by simulation studies based on a 
dynamic model consisting of a two-link flexible joint robot with two joint motors. 

1. Introduction 

The development of robotics in the past few years has been extended from the earlier 
standard applications of industrial robots to new fields such as service, space robotics and 
force-feedback systems. The design goals of the new robot generation aim at lightweight, 
high output torque, high speed, multi-sensory and high degree of learning capability. Such 
advanced features inevitably increase the complexity of the dynamic control tasks. For a 
lightweight robot, to avoid the disturbance torque, such as backlash etc., gearboxes with 
harmonic drive are often involved; this leads to flexibility in robot joints in turns. It is 
recognized meanwhile that the dynamic control of real world lightweight robots to reach a 
high system bandwidth is a challenging topic to the current development of robotics and 
available control technologies. The key factor which limits the system bandwidth is the 
"high-order", originated from the joint flexibility and the dynamics of the electric motors. 
It is recognized that the state-space approach based on the feedback linearization is not 
adequate for the control of real world lightweight robots and even not adequate for the 
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control of any high-order non-linear uncertain system, despite being able to assign the 

closed-loop poles arbitrarily. Another methodology to control the lightweight robots is to 

decompose the high-order system into two or more lower order sub-systems. There are 

some remarkable advantages with this methodology: control approaches for rigid-body 

robots may be used further; the higher order time derivatives of the link position such as 

acceleration and jerk may be avoided; and, it is easier to set the control system into 

operation. One of the control methods under this category is the famous singular 

perturbation approach (as well as the integral manifold approach) which takes the joint 

torque sub-system as an algebraic system for the link position control and adds some 

damping for the fast motion in the joint torque. In this way, the joint torque dynamics are 

resolved without the need of exact tracking of a joint torque reference trajectory. Because the 

joint torque dynamics are almost "by-passed", this approach may possess a higher 

bandwidth for the link position control than the pure cascaded control structure with a joint 

torque control loop being inserted between the link position and motor current control loops. 

As a result, the composed control structure of singular perturbation approach for the joint 

torque dynamics can be interpreted as a feed-forward control of the joint torque added by 

some damping to the fast motion in the joint torque. Singular perturbation approach is 

verified as a simple and effective approach to stabilize the joint flexibility. 

A pioneer of flexible joint robot control is Professor Mark W. Spong when he worked for 

University Illinois froml984 to 2008. He established the famous Spong-model for flexible 

joint robots and studied almost all aspects for the dynamic control of this kind of robots. 

In the following, some important publications will be citied to clarify the main stream of the 

dynamic control issues. 

The concept of new generation robotics with modular structure was proposed by 

Hirzinger's group as the spring-out of space robotics technologies (Hirzinger et al., 1994; 

Gombert et al., 1995). Later on, the concept was modified to the goals of having human arm 

performance with very high load/ own-weight ratio as well as torque sensing and feed-back 

capability, with certain degree of human intelligence, providing new possibilities for space, 

medicine and other applications (Stieber et al., 2000; Schmidt, 2000; Hirzinger et al., 2001; 

Hirzinger et al., 2001; Koeppe & Hirzinger, 2001). 

The fundamental control approaches for flexible joint robots were established by Spong 

(Marino & Spong, 1986; Spong, 1987; Spong, 1988; Spong, 1989). Since then, numerous 

theoretical results are developed and mainly tested with computer simulation. The 

developed control methods include: 

(a), state-space approach based on the feedback linearization 

(b). singular perturbation approach as well as integral manifold approach 

(c). dynamic feedback linearization approach 

(d). adaptive control technique 

(e). simple PD control 

(f). PD control + joint torque feedback 

(g). passivity based control approach 

As proposed in (Spong , 1987), for the state-space approach based on the feedback 

linearization, even using simplified robot model, the resulting control algorithm may not be 

realizable due to the state transformation and the inverse calculation of the control inputs. 

The control algorithm depends on the robot parameters, which are generally unknown. 
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As stated before, singular perturbation approach is a promising approach by solving the 
control problem in two time scalars: a fast joint torque damping term for the fast mode of 
the joint torque dynamics, and a slow joint torque feed-forward term for the outer position 
control loop (related to the rigid body dynamics of the robot arm) (Spong, 1987; Readman & 
Mark, 1994). 

De Luca involves the previous system information to form the so-called dynamic feedback 
linearization (De Luca et al., 1998). He uses not only the actual states of the robot dynamics, 
but also the past states; no global state transformation is required. The resulting control 
structure is of 2n(n-l) order (with n being the number of robot joints). (De Luca et al., 1998) 
won a best paper awarded during conference IRCA98 due to the theoretical contribution. 
In order to remove the requirement of exact knowledge about robot parameters, adaptive 
control techniques for flexible joint robots have been developed (Spong, 1989, Lin et al., 
1995). These approaches can be viewed as an extension of adaptive control for rigid body 
robots (Slotine & Li, 1987). Though theoretically looks well, this method met the problem of 
over complexity for the practical implementation. 

Engineers tried PD (or PID) controllers, traditionally used for industrial robots, adding some 
damping term for the joint flexibility. Stability proof for such control systems, if it is possible, 
is more involved than that of using extensive model information. Starting from (Arimoto, 
1994), which provides the theoretical justification for the PD controller still used in most 
industrial robots, Tomei (Tomei, 1991) proved the stability of PD control with gravity 
compensation also for flexible joint robots. However, the stability proofs are only valid for 
the link position regulation and not for the trajectory tracking control. 

Albu-Schaeffer (Albu-Schaeffer & Hirzinger, 2000) proposes an intermediate approach 
between the theoretical and the practical solutions for the link position control i.e. PD 
control + joint torque feedback. He uses a simple control structure in the form of joint state 
feedback with gravity compensation, applicable for a lightweight robot with 7DOF. A 
stability proof based on Lyapunov theory was provided as well. Also here, the stability 
proof is valid only for the case of point-to-point motion of the robot arm and not valid for 
the trajectory tracking control. 

Ott (Ott, 2008) studied and tested several control approaches systematically including the 
passivity based control approach. It comes to the conclusion that the passivity based control 
approach doesn't show an improved performance for the trajectory tracking control despite 
of some other advantages. Similar to the works by Albu-Schaeffer (Albu-Schaeffer, 2002), the 
proposed control algorithms by Ott need often the system parameters which may not be 
available for general purpose lightweight robots. 

In (Ozgoli & Taghirad, 2006) an extensive survey about the control of flexible joint robots is 
given in which 173 papers from different aspects of the control issue are cited. 
It is recognized meanwhile that to design a good control system, the controller designer 
must have a deep understanding about the physic plant to be controlled, independent from 
which control approach is applied. As a result, at least a rough model for the controlled 
plant is required, though there are some unmodeled dynamics, external disturbances and 
parameter uncertainties associated with this rough model. As a candidate of physic oriented 
control theories, sliding mode control (Utkin et al., 2009) is selected here for the control 
problems of flexible joint robots. As it well known, sliding mode control theory can be 
applied to high-order, non-linear, uncertain MIMO systems and the resulting controllers are 
simple enough for practical implementations. Another advantage of sliding mode control 
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theory is easy to understand for normal control engineers (it is the main reason why this 
control theory becomes more and more popular). The major disadvantage associated with 
sliding mode control is the chattering phenomena due to the high frequency switching of 
the discontinuous control input. However, if the chattering problem can be solved or the 
inherent discontinuous property of the plant actuators (like electric motors) can be 
positively utilized, sliding mode control theory will be a good design tool for deriving the 
control algorithms. In this chapter, the design methodology of sliding mode control will be 
the major theoretical tool for the control of flexible joint robots. 

The rest of this chapter is organized as follows: 

In Section 2, the control problems for rigid-body robot manipulators with modelling 
uncertainties and external disturbances will be dealt with. The resulting control algorithm 
will be used for the link position tracking control of flexible joint robots. Section 3 handles 
the joint torque dynamics based on the singular perturbation approach. We use the result of 
other researchers without repeating the theory of singularly perturbed systems. Section 4 
presents the theoretical derivation of sliding mode PWM for the current control of PMSM. 
This current controller will be used as the most internal control loop for the link position 
tracking control. Section 5 shows the simulation study, verifying the developed control 
algorithms, based on a dynamic model consisting of a two-link flexible joint robot with two 
joint motors. In section 6 some conclusions will be given. 

2. Robust control of rigid manipulators based on integral sliding mode 

2.1 Problem statement 

For rigid body robot manipulators, the computed torque approach provides asymptotic 
stability for tracking control tasks. However, the state dependent matrices needed to 
complete the computed torque algorithm are normally unknown and possibly too complex 
for a real-time implementation. This section proposes a simple controller with computed- 
torque-like structure enhanced by integral sliding mode, having pole-placement capability. 
For the reduction of the chattering effect generated by the sliding mode part, the integral 
sliding mode is posed as a perturbation estimator with quasi-continuous control action 
provided by an additional low-pass filter. The time-constant of the latter tunes the controller 
functionality between the perturbation compensation and a pure integral sliding mode 
control, as well as between chattering reduction and system robustness. 

Studies on the control of chain-like mechanical systems have been a subject of intensive and 
profitable research over the last three decades. Robot manipulators, as dynamically coupled 
non-linear MIMO systems have attracted the attention of many control scientists and 
engineers. Arbitrary assignment of the system poles of a set of decoupled and linearised 
sub-systems has been the final design goal. The computed torque (Hunt et al., 1983; Gilbert 
& Ha, 1984), as a theoretically simplest and most comprehensive approach for the tracking 
control of robot manipulators, allows one to assign the poles of the closed-loop system 
arbitrarily at the price of an exact feedback linearization with state dependent quantities for 
compensation of the system non-linearity with coupling terms. Any mismatch due to 
parameter or modelling uncertainties in the plant will violate exact linearization and 
decoupling. Moreover, even when these quantities are known exactly, the real-time 
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implementation is still an issue, since the computational overhead might be too large to 
prevent the control algorithm from being realized in control hardware. 

Motivated by the recent developments on integral sliding mode control (Utkin & Shi, 1996; 
Poznyak et al., 2004; Cao & Xu, 2004; Castanos & Fridman, 2006; Utkin et al., 2009), by 
taking regard on algorithm complexity, this section proposes a novel control structure with 
pole-placement capability for rigid body robot manipulators. Simple matrices describing the 
nominal model (normally they are constant, as long as the available joint torques are high 
enough) are used to form a computed-torque-like controller, whereas two diagonal control 
gain matrices are responsible for the pole-placement. In addition, an additive control vector 
is designed based on the concept of integral sliding mode to compensate for the overall 
matched system uncertainties (for systems with unmatched uncertainties, other than the 
case of full actuated robot manipulators, the readers are referred to (Cao & Xu, 2004; 
Castanos & Fridman, 2006)). 

Control of robot manipulators using sliding mode technique has a rather long history. Since 
the first set-point sliding mode controller suggested by (Young, 1978), numerous variations 
have been proposed in the literature, such as the component-wise control discussed by 
(Slotine, 1985) and by (Chen et al., 1990). The robustness property of the conventional 
sliding mode control with respect to variations of system parameters and external 
disturbances can only be achieved after the occurrence of sliding mode. During the reaching 
phase, however, there is no guarantee for robustness. Integral sliding mode aims at 
eliminating the reaching phase by enforcing the sliding mode on the entire system response 
(Utkin & Shi, 1996). As a result, robustness of the system can be guaranteed starting from 
the initial time instant, that is, a robot manipulator is able to track the reference trajectory 
(with designed error dynamics given by the pole placement) throughout the entire system 
response despite the system uncertainties. 

However, since a discontinuous term appears in the resulting joint torque, direct 
implementation of the integral sliding mode control algorithm may be difficult due to the 
chattering effect. To solve this implementation problem i.e. to reduce the chattering level, the 
discontinuous term is used for a perturbation estimator based on an auxiliary internal 
dynamic process. It will be shown that the equivalent control of such a discontinuous term 
is indeed able to compensate the net system perturbation. 

If the equivalent control could be obtained exactly, the system perturbation could be 
compensated for completely, so that the system would be free of chattering and robust 
starting from the initial time instant. Strictly speaking, the exact equivalent control based on 
the system model is impossible to achieve, primarily due to model uncertainties. However, 
if the spectrum of the equivalent control has no overlap with the switching frequency of the 
discontinuous control term (it is normally the case in practice), a low-pass filter can be used 
to extract the equivalent control from the discontinuous control term (Utkin, 1992). Using 
low-pass filter to extract equivalent control from the discontinuous control term provides 
the basic information source of proposed control design. 

From the practical point of view, the bandwidth of the low-pass filter is designed as low as 
possible, so that the amplitude of the chattering remains low level. However, since the 
frequency of the equivalent control is time-varying, a low-pass filter with a fixed time- 
constant and low bandwidth would "cut" the equivalent control and lose the information 
about the system perturbation. Thus, there is a trade-off between the system robustness 
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(whether the system perturbation can be compensated for completely) and the chattering 
reduction by tuning of the time-constant of the low-pass filter. 

2.2 Integral sliding mode control and perturbation estimator 

In this section, the basic concept and the main result of integral sliding mode control will be 

outlined. 

For a given dynamic system represented by the following state space equation 

x = f(x) + B(x)u + h{x,t) (1) 

with x e S J!" being the state vector, u e 5R" being the control input vector ( rank B(x) = m) and 
h(x,t) being the perturbation vector due to model uncertainties or external disturbances; 
h(x,t) is bounded and assumed to fulfil the matching condition. The control low for system 
(1) is proposed as 

u = u n + u 1 (2) 

where u e W is responsible for the performance of the nominal system; u, e SR" is a 

discontinuous control action that rejects the perturbations by ensuring the sliding motion. 
The sliding manifold is defined as 

s = s (x) + z, 

with 
s, s (x), z e s Ji'" 

ds ( 3 ) 

z = --±{f(x) + B(x)u (x)} 
ox 

z(0) = -s (x(0)) 

where initial condition z(0) is determined under the requirement s(0) = . It can be proven 
that the equivalent control of u t will cancel out the perturbation term h(x,i) , see (Utkin et al. 
2009). Discontinuous control u l has a proper selected control gain which ensures sliding 
motion starting from t = i.e. s(0) = . 

In real applications, however, discontinuous control u t may result in chattering effect, 
imposing high frequency vibrations. To reduce this undesired effect, the control system can 
be modified as follows: 

s = s (x) + z 

z = -f/(x) + B(x)u -B(x)uA 

dx 

z(0) = -. (x(0)) ( 4 > 

u = u + u Un , 
u lav =lowpass(u ] ) 
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By solving equation s = for u l , it can be directly checked that the equivalent control of u 1 
still cancels the system perturbation. In the above controller, relation u lei = u lm is used, for 
proof see (Utkin, 1992). Finally, the term u Im , is quasi-continuous (depending on the time- 
constant of the low-pass filter) and equal to the perturbation term to be compensated for, 
serving as the perturbation estimator. Moreover, since discontinuous control u l appears 
only in the control computer, its gain is more flexible to tune. 

2.3 Control of robot manipulators 

2.3.1 Model of rigid body robot manipulators 

The model of a rigid body robot manipulator with n degrees of freedom can be written as 

M(q)q + C(q,q)q + G(q) + F(q) = T (5) 

where M(q) eS"" is the mass matrix; C(q, q)q eW is the vector including centrifugal and 
Coriolis forces; G(q) s 9T is the gravity force vector; F(q) e s Ji" is the friction force vector; 
q e 3?" represents the joint position vector and reM" denotes the joint torque vector. 
For the purpose of control design, the notation of the above model can be formally changed 
to 

M(q)q + N(q,q) = T (6) 

where vector N(q,q) = C(q,q)q + G(q) + F(q) does not contain term q . This model can be 
rewritten as the sum of an ideal model and a perturbation term: 

M (q)q + N (q,q) = r + H(q,q,q) (7) 

where M (q) = M (q) - AM , N a (q,q) = N(q,q)- AN , with AM and AN being the unknown 
part of matrix M(q) and vector N(q,q) , respectively; vector H(q,q,q) denotes the overall 
system perturbation and has the form H(q,q,ij) = -(AM q + AN) . Note that the perturbation 
term H(q,q,q) satisfies the matching condition. 

2.3.2 Control design using integral sliding mode 

Following the design principle given in section 2.2, the joint torque vector r can be 
designed as two additive terms: 

T = T a + T, 

(8) 
h =M (q)(q d -K D q e - K r q r ) + N (q,q) 

where M (q), N (q,q) are the nominal value of M(q), N(q,q) , respectively, as defined 
with equation (7); K p e S JT*" , K D e 9?"*" are positive definite diagonal gain matrices 
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determining the closed loop performance; and the tracking error is defined as 
<7 S (0 =< 7(0 — Qi(t~) with [g,,(0 q,i{t) q<i(t)] being the reference trajectory and its time 
derivatives. Note that r represents the computed torque part of the controller. 
Discontinuous control r, is now derived based on the design principle of integral sliding 

model control: 

Step 1: Sliding Manifold 

The sliding manifold is defined based on equation (3) 



i 


= s (x) + z 




s =[C I] 


>1 




' I] 


_-M-'N +M ] t 


-&- 


z(0) = 


—Cq.(0) 


-*.(0) 





(9) 



where C e 9?"*" is a positive definite gain matrix and I e W" is a n x n unit matrix. 
Vector s can be further simplified by substituting r with equation (8): 



= ?.+X- D 8r.+^,jg.(5)^-«.(0)-X 1>? .(0) 



(10) 



Since the requirement ^(0) = is satisfied, sliding mode will occur starting from the initial 

time instant t = . Note that for the implementation of s , matrix C is not required in the 

final equation, see (10). As one can see from the derivations given above, equation (10) is the 

natural extension of the basic design equation of integral sliding mode (3). 

To prepare the stability analysis, the time derivative of the sliding variable s(t) can be 

obtained 



:i 0+ i = Cl+C2 Z "o+ Ar ' r i 



(11) 



where £", =(Mq 1 N -M~'N) and £ 2 =(M~' -M ') represent the mismatches between the 
nominal parameters M (q), N {q,q) , and the real system parameters M(q) , N(q,q) , 
respectively, viewed as system perturbation terms. Note that in this study we assume that 
both £ and £ 2 r are norm-bounded. 
Step 2: Discontinuous control r, 

r, is the discontinuous control dedicated to reject the overall perturbation torque H(q,q,q) . 
Here r, can be selected as 
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s 



r '=- r °0 ( 12 ) 



■ s 



where T is a positive constant (control gain may also take other forms) and p denotes the 
norm 2 of s i.e. \\s\\ 



Step 3: Design of the control gain T 

1 T 

Select a Lyapunov function candidate as V =—s s>0 (for s # ). The time derivative of V 
along the solutions of (11) is given by 

V = s T s= s T (Cl + C 2 r ) - r /M-^/H (13) 

Since matrix M ~ (q) is positive definite and T is a positive constant, the most right term 
in (13) i.e. T s M~ s/H is positive for any s # . For a small enough positive number p , 
such that inequality F s M~ s/|s| ^ r i P' s /||' s || holds, it can be shown that 

^-pKrop-lld + ^rol) (14) 

Clearly, under the norm-boundedness condition of terms ^j and ^2 r 0' a l ar g e enough gain 
r can always be chosen to guarantee V < -a lis (with a > and for lis * ), implying the 

occurrence of sliding mode in finite time. Note that the initial conditions in (10) eliminate 

the reaching phase. 

Step 4: Equivalent control of T\ 

Once sliding mode occurs and the system is confined to the manifold s(t) = , the 

equivalent control of X\ can be used to examine the system behaviour. The equivalent 

control is obtained by formally setting s = , yielding 

r w =-M(<r 1 + ^ 2 r ) (15) 

Substitution of r = r + r 1(? „ in equation (6) with equivalent control (15) leads to the motion 
equation in sliding mode, which can be simplified as 

M (q)q + N (q,q) = T (16) 

Control r in (8) thus achieves the designed (closed-loop) error dynamics defined by K D 
and K P , namely 

q e +K D q e +K P q e =0 (17) 
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as if perturbation term H(q,q,q) in (7) would not have existed. Equation (16) as well as (17) 

represents the system motion in sliding mode. Solving q from (16) and setting into 

H(q,q,q) , easily shows the perturbation cancellation property, i.e. r lt ,„ =-H(q,q, q) . The 

derivation above is only to show the perturbation cancellation property by the equivalent 

control Tj . Actually, the designed closed loop motion presented by (17) can be obtained 

more easily by taking the time derivative of (10) and set i = . 

Summarization of the integral sliding mode control system for the implementation: 

?"o =M a (q)(q d -K D q e -K P q e ) + N (q,q) 
i 
s = q e +K D q e +K P jq e (g)dt-q e (0)-K D q e (0) 

o (18) 

s 

\s\\ 



h=~ T o 



T = T + Tl 

From (18), one can see the benefit of the control system: in order to assign the poles of the 
closed-loop system arbitrarily, one needs only to additionally calculate the variable s and 
t\ , exact knowledge about M(q) and N (q, q) are not required. Depending on the available 
control resource, the nominal quantities M (q), N (q,q) can even be set constant i.e. to 
M Q , N q . Moreover, the robustness of the tracking control performance is ensured starting 
from t = . 

2.3.3 Control design using integral sliding mode based perturbation estimator 

Hitherto, the control system described in section 2.3.2 looks perfect. However, in some 
practical applications, the controller given in (18) may not be applicable to robot 
manipulators, as the chattering level generated by the discontinuous control term T\ may be 
very high. Following the control design approach given by (4), the control system can be 
modified to: 

?0 = M (q)(q d - K D q e - K P q e ) + N (q,q) 

t t 

s = q e +K D q e +K P ]q e (Z)d%-q e (0)-K D q e (0) + JM l (q)(T l -T lav )dZ 

o o 

(19) 

T \av =lowpass(r l ) 
r = r + r lav 
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Note that for a better decoupling, the control gain of r l may also be selected as M r 

instead of F . However, since we are intended to compare the solution based on the 

perturbation estimator with the pure integral sliding mode control (18), the control gain is 
designed to have the same form for the both control systems. Now, the equivalent control of 
T\ can be obtained by setting s = 

i = s + z = Ci + Ci T + Mn t\ = 

(20) 

Actually, since r = r + r lav = r + T le , (20) can be further simplified to (15), implying that 

the equivalent control t 1 remains the same as in the case of pure integral sliding mode 

control. 

For the convergence proof of s to zero, check that the dynamic motion about s in the 

closed-loop system can be derived as 

s=s a +z = <; l +£ 2 T-M (i X T l (21) 

1 T 

For a Lyapunov function candidate V = — s s > (for s ^ ), the time derivative of V along 
the solutions of (21) can be obtained as 

V = s T s = s T (Ci + GO - r</Mo~VH W 

Similar lines as in (14) can be followed to show that a large enough control gain F can be 
selected such that V < -a lis (with a > and for |U ^ ), implying that sliding mode will be 
enforced in finite time. Note that t in (22) is now quasi-continuous due to the low-pass 
filter, it can be assumed here that terms £j and ^ 2 r are norm-bounded. Again, initial 
conditions guarantee that s(0) = in (19), thus eliminating the reaching phase. 
The advantage of controller (19) over the previous controller given by (18) is: the 
discontinuous control term r l (with gain T ) appears only in the control computer and the 
real control r applied to the robot manipulator (see (19)) is low-pass filtered. Control term 
z"i flv serves here as a perturbation compensator. As one can see from (19), if the time 

constant of the low-pass filter tends to zero, the controller given by (19) will converge to 
controller (18), i.e. from perturbation estimation solution to integral sliding mode control 
solution. For the control system under controller (19), sliding mode s(t) = is guaranteed 
throughout the entire system response, although a low-pass filter is involved in the control 
loop. 

2.3.4 Practical consideration 

Since low-order filters do not ideally cut off the high-frequency switching signal 
components due to the discontinuous term rj , some amount will be still preserved in r lav . 
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Whereas for practical applications, a large time constant for the low-pass filter is normally 
preferred, such that the resulting control signals remain as smooth as possible. However, 

since the instantaneous frequency of the system perturbation (i.e. the frequency of T, after 

sliding mode occurs) is unknown and time changing, it may happen that the bandwidth of 

rj is higher than the bandwidth of the low-pass filter and the system perturbation cannot 

be cancelled out completely, thus the system robustness is reduced. For a high control 
performance, the time constant of the low-pass filter should be made small (at least during 

the transient period) such that the bandwidth of the low-pass filter is high enough and T leq 

can get through the filter completely. 

As a result, in the practical implementation the time constant of the low-pass filter can be 
used as a trade-off between chattering reduction and system robustness: if a high robustness 
as well as high control accuracy during the transient period is required, the time constant of 
the low-pass filter can be made small for the short time period. The trade-off between 
chattering reduction and system robustness by changing the time constant of the low-pass 
filter is demonstrated in Sections 5.2 and 5.3. 

3. Singular perturbation approach to handle the joint flexibility 

As mentioned in the introduction part, singular perturbation approach has at least the 

following advantages: 

(a), the signals for the control implementation can be made available 

(b). there is no need to implement an exact tracking controller for the joint torque 

(c). the results for the control of rigid-body robots can be used further 

(d). the implementation of the control algorithm is easy 

Sure, singular perturbation approach has also disadvantages: 

(a), it is not valid if the joint stiffness is too low 

(b). the control law is sensitive to the change of joint stiffness 

Fortunately, most of lightweight manipulators used in practice have high enough and fixed 

joint stiffness. The flexibility in robot joints is a side-effect to achieve lightweight and it is 

normally not intended by the robot designer. 

The control algorithm of this section will be summarized here without repeating the theory 

of singularly perturbed systems. The way of treating the joint torque dynamics can be find 

e.g. in (Ott, 2008). 

The output of the robust link position controller for rigid body manipulators given in 

Section 2 is denoted here as r d (instead of r ), which is the reference input for the joint 

torque implementation. Normally, when using singular perturbation approach for the 

control of slow dynamics, the joint inertia matrix J has to be considered in the link position 

controller by adding matrix J to the mass-matrix of the robot arm M(q) . However, since 

our link position controller is a robust controller, implying that no exact parameters are 

required, the information about the joint inertia is normally not necessary (the system 

robustness depends on the available control resource). 
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The reference current vector for the joint motors can be calculated from the slow and fast 
torque components i.e. r s eR" and Tf-eR" for stabilizing the joint torque dynamics 

ll=K7 l r m =K; l G-\v s + T f ) (23) 

where K t is the diagonal torque constant matrix of the electric motors and G,. is the 
diagonal gear-ratio matrix; I„ =[i qj ]eR" , with i = \~n, is the reference current vector 
including the reference currents for all joints; r m represents the motor torque vector. The 
slow and fast joint torque components can be given as 



r f =-D T i 
or 

Tf=-K T (T-T d )-D T T 



(24) 



with D T e R" " and K T e R nxn being constant diagonal control gain matrices to be 
determined by the control designer (if the joint stiffness is changed the control gain matrices 
need to be retuned accordingly). 

4. Direct current control using sliding mode PWM 

When using the build-in PWM unit of a micro-controller or a DSP, the required reference 

voltage signals generated by the current controller will be modulated in form of pulse- width 

and then it is hoped that the average value of the terminal voltages of the stator windings 

will be equal to the reference voltages that the current controller produces. In this 

configuration there are two problems: 

(a), the PWM implementation of the terminal voltages is done in a way of open-loop, the 

final voltages on the stator windings may differ from the ones what current controller 

requires, depending on the quality of the pulse-width-modulation. 

(b). it introduces some time delay, at lease a duty-cycle has to remain unchanged before the 

corresponding PWM signal being sent out. 

Thus for a high dynamic performance, the build-in PWM unit of a micro-controller or a DSP 

has some disadvantages. 

On the other hand, the conventional current control hardware such as Chopper-Control or 

Hysteresis-Control hardware do not have these disadvantages. Because no micro-processor 

being available, these practically used hardware were not able to implement the concept of 

field-oriented control. In this section we derive a current controller based on sliding mode 

control theory for PMSM which has the performance of field-oriented control, but without 

the disadvantage associated with the open-loop PWM techniques. We call this kind of 

current control "sliding mode PWM current control". 

At first, we need the motor model to design the current controller. The motor model in the 

( d, q )-coordinate frame, which rotates synchronously with the motor rotor, can be given as 
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di d 

L = u d —Rij + LcoJ,, 

dt q 

A. (25) 



dt 



-u q -Ri cl -L(o e i d -A co e 



where L is the stator inductance and R is the stator resistance; i d and *' are the stator 
currents in the (d,q) coordinate frame; u d and u„ are the stator voltages in the same 
coordinate frame; Aq is the flux constant of the rotor permanent magnet; w e is the rotor 
electric angular speed. 

For the sliding mode current controller, the switching functions for the d and q current 
components are designed as 

.* 
s d = 'd - 'd 

(26) 



where i. is the reference current i.e. one of the components of the compose controller (23) 

(index i is neglected here for simplicity), and reference current component i d = for 

constant torque operation and i d ^0 for field-weakening operation (Shi & Lu, 1996). The 
time derivative of both switching functions along the solutions of (25) can be found as 



: :* 1 R . . :* 

s d =l d~ l d =— u d ~~T l d +a) e l q ~ l d 

1 R. Ao 



(27) 



S q ~ l q~ l q ~T U q ~T'l ~ ° }e ' d ~~T W e ~ l q 



Introducing two auxiliary variables f d and f„ as follows 



, R . . :• 

Jd = ~T ld +C ° el i ~ ld 



(27) will be simplified to 



R Aq •* 

■— l q-<°e l d - — co e-'q 



' s d =fd+ L l "d 
s q = J q + L u q 



(28) 



(29) 



The above equation system can be summarized in vector form, resulting in 
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■jrf 




~ri 


+{- 1 


u d 


Lv 




J*. 




_ u q_ 



(30) 



Here stator voltages u d and u„ are not yet the discontinuous voltages applied to the stator 

windings. For the sliding mode current control we need the relationship between the final 
discontinuous voltages applied to the stator windings i.e. uj ~ u 3 (which take the values 
from the set {-w ,w } with « being the DC-Bus voltage) and the time derivative of both 
switching functions. This relationship can be given as 





= 


7/ 


+ r l 


u d 

u q _ 


= 


7/ 



d,q 



(31) 



.1,2,3 



where matrix A J ' can be expended as 



,1,2,3 
%q 



co$6 a cos# 6 cos# 
-sin# a -smOfr -sint 



(32) 



with 6 a =9 e , ffj, = e —2tcI3 , C = e +2x/3 and e being the rotor electrical angular 
position. Using (32), (31) can be rewritten as 



fd 



uy cos 9 a + u 2 cos 9 b + w 3 cos C 
~u x sin a - u 2 sin b - n 3 sin C 



(33) 



To find the control signals «j , u 2 and w 3 , Lyapunov approach can be employed. Design a 
Lyapunov function candidate as 

' • ' (34) 



: s <k s M 



where S tl = [s d s ] T . The time derivative of V along the solution of (33) can be found as 



v=s' dq s dq 



•■['< *,] 



(35) 



-L-'[s d 



M, COS a + U 2 COS h + M, COS t 

-«j sin 6? a - « 2 sin 6 h - u i sin t 



which can be further expanded to 
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(36) 
= tedfd+ s J q ) + L [«i ted cos #« " *, sin B a ) + u 2 (s d cos 4 - s q sin 6> 4 ) + K3 (s rf cos 6> ( , - 5, sin 6> £ , )] 

Introducing the following three auxiliary variables 

Oj = (sj cos 9 a - s sin a ) 

n 2 = (^ cos h - s q sin b ) (37) 

f2 3 = (sj cos # - 5 sin 6 c ) 

equation (36) can be simplified to 

y = tej d + s q f q ) + L- 1 (up, + u 2 n 2 + up 3 ) (38) 

In order to guarantee V < , the control signals u x , u 2 and u 3 can be designed as 

w, = -u sign(Q. l ) 

u 2 = -u sign(Cl 2 ) (39) 

w 3 = -u sign(Q. 3 ) 

With these notations, equation (38) can be reformulated for the final analysis 

y = te d fi +s q L)-L'\[sign(n l )n, +sign(n 2 )n 2 + sign(n 3 )n 3 ] 

(40) 

= teJ d +\f q )-L-'u [p l \ + p 2 \ + \Q 3 \] 

In the above equation, 17 is a constant (but may be unknown). If the scalar term 

tedfd +s fa) ls bounded and if the DC-bus voltage u Q is high enough, V <0 can be 

guaranteed, implying that the real currents will converge to their reference counterparts in 

finite time. Thus the stability of the current control system can be ensured under two 

conditions 

(a), the DC-bus voltage u is high enough 

(b). auxiliary variables f d and f are bounded 

Since f d and /„ do not contain the control voltages, neither u d and u q , nor mj , u 2 and w 3 , 

the condition (b) is reasonable. Note that if the reference currents i d and i change too fast, 

the stability condition may be violated from time to time (depending on the available DC- 
bus voltage u ). In this case there exists no current controller which can do better. Some 
researchers design sliding mode link position controller with discontinuous joint torque 
commands and without taking into account the motor dynamics, would meet this problem. 
Other high gain link position controllers without taking into account the motor dynamics 
would meet the same problem. 
Now the implementation procedure is summarized. 
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Though the derivation of the proposed current controller looks rather involved, the 
implementation of this controller is quite simple. The equations for the implementation are 
summarized as follows 

* Q., = (.?,cos# -s sin ) 
s d = i d -i d 

„/ fi 2 =(s d cosff t -s t sin b ) , 

S i~ l i 'i a.=(s d cos0-s a sin0 c ) 

(41) 
"i = -u s'gn(Q,) 

u 2 = -u sign(Cl 2 ) 
w 3 = -u sign(fl 3 ) 

with O a =0 e , 0j,=0 e -2ft/3, C =0 e +2n/3 . The final gating signals taking values from set 
{0, 1} (like PWM signals) feeding to the inverter can be found as 

s wl = 0.5(1 + «i/« )/ 

s w4 = 1 — s wl i 

s w2 = 0.5(1 + u 2 /u ), 

(42) 

' s u-5 - l _ s w2 i 

s w3 =0.5(l + u 3 /u ) , 

s w6 = 1 ~ ' s ic3 ■ 

The switching control signals s wl ~ s w6 are pulse signals, the pulse width is not calculated 

from some duty-cycle, but determined directly and instantaneously by the current control 
errors in the field-oriented coordinates. Note that in practical implementation, several /is 
time delay is required between signal pair s wi and s wi+3 (i = 1~ 3 ). This current control 
system does not require the motor parameters as well as the decoupling process, thus it is a 
robust current control system. 

5. Simulation Studies 

5.1 A two-link robot manipulator as an example 

A planar, two-link manipulator with revolute joints, taken from the example in (Utkin et al., 
2009), is used here to demonstrate the proposed control approaches. The manipulator and 
the associated variables are depicted in Figure 1. 
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Fig. 1. Two-link manipulator with link lengths L x and L 2 / ar, d concentrated link masses M l 
and M 2 . The manipulator is shown in joint configuration (gj, q 2 ) , which leads to end- 
effector position (x w ,y w ) in world coordinates. 

The end-effector position, (x w , y w ) , i.e. the location of mass M 2 in world coordinate frame 
(x, y) , is given by 

x w = L\ cos qy + L 2 cosiqy + q 2 ), 

y w = L x sin q t + L 2 sin(q l + q 2 ), 



(43) 



where (q\, q 2 ) denotes the joint displacements and L\, L 2 are the link lengths. Solving (43) 
for the joint displacements as a function of the end-effector position (x w , y w ) yields the 
inverse kinematics as 



?2 =atan2(AC), with C = %w +yw -^ ^ 2 , D = ±^1^ 

2L X L 2 

q\ = atan2(>^, x w ) - atari 2(Z 2 sing 2 , L\ +L 2 cosq 2 ) 



(44) 



which obviously is not unique due to the two sign options of the square root in variable D. 
The function "atan2( . )" describes the arctan function normalized to the rangel80. 
The dynamic model of the two-link manipulator can be given as 



'21 m 22 



M(q): 



m 21 m 22 



c \+g\+fl 
c 2 +g2+fl 

N{q,q)-- 







T\ 






= 




,i.e. 


l\ 




J2_ 




~ c \+g\+f\~ 


c 


2 + 


gl- 


^fi\ 



(45) 



with 
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m 12 = m 21 = m 22 + L\lqM<i COS q 2 > 
mj j = Zj (Mj + M 2 ) + 2m l2 — '"22 > 
c l = -L 1 L 2 M 2 (2 g^ 2 - g| ) sin q 2 , 
c 2 = L x L 2 M 2 q x sing 2 , 
g 2 =L 2 M 2 gcos(q 1 +q 2 ), 
g\ =L l {M l +M 2 )gcos{q]) + g 2 , 
A =k v l9i+k c isign(q 1 ), 
h = k v2<J2 + k c2 si g n (q2)> 



(46) 



where k vi and k ci ( / = 1,2 ) are coefficients of viscous friction and coulomb friction, 

respectively. 

The joint model for the two robot joints is given by 



J fii + T dsi + T i = g,-i T mi 
T^K^-qJ J = 1-2 



(47) 



,-ih 



joints are 



T ■ ' 

mi ' 
T dsi '■ 
Ji- 

K,: 



where the parameters and variables for the 1 ' 
q, '■ link position 
9j '■ joint position 
joint torque 
motor torque 
disturbance torque 
joint inertia 
joint stiffness 
g,i '■ gear ratio 
The electric motor model for each joint is taken from equation (25) with the transformation 
matrix given in (32). 

The plant parameters for the simulation study are selected as shown in Table 1 through 
Table 3. Note that for the simulation, we select the joint disturbance torque in equation (47) 

as pure viscous friction z dsj = k coj 6 i for both joints (but at link side both viscous and coulomb 
frictions are applied, see equation (46) and section 5.2). 



Table 1. Arm Parameters 



My 


M 2 


k 


L 2 


2 kg 


1kg 


0.5 m 


0.5 m 
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L (H) 


R(Ohm) 


4) (Wb) 


P 


^, (Nm/A) 


q max \ / 


« (V) 






22.5 xl(T 3 


0.78 


0.26 


4 


(3/2)Pio 


50 


100 




Table 2. Parameters for motor 1 and motor 2 (P = number of pole-pair) 








J(Kgm 2 ) 


K (Nm/Rad) 


£r 


£ ffl (Nm/(Rad/s)) 






1.0 


12000 


20 


1 





Table 3. Parameters of joint 1 and joint 2 

For the trajectory tracking control task, we will demand the manipulator to follow a circular 
trajectory in its workspace. The circle with centre (x d0 , y d0 ) and radius r d is given in world 
coordinates by 

x d( t ) = *d +''cl c °Wd 

yd{t) = yd- r d^¥d 



Wdit)- 



2n 



2k 
— t 



(48) 



0<t<t 



f> 



where the operation is assumed to start at time t = and to be completed at final time t = tr. 

Through the inverse kinematics, the reference link angles for joint 1 and joint 2 are 
calculated according to (44). The parameters for the reference trajectory are chosen as shown 
in Table 4. 



x d0 


ydo 


r d 


'/ 


0.25 m 


0.25 m 


0.5 m 


2s 



Table 4. Parameters of reference circular trajectory. 

5.2 Controller parameters and simulation configuration 

The parameters for the outer link position control loop are selected as: 



K„ 



N {q,q) 

100 
100 



"2.5 0" 


1 




~°1 


)- 


LoJ' 



,K d 



20 
20 



400 
400 



(49) 



The joint torques of both joints are limited to 400Nm. To extract the equivalent control from 
the discontinuous control term to obtain rj flv = X\ eq in equation (19), a simple first order low- 
pass filter is used i.e. 



Control of Lightweight Manipulators Based on Sliding Mode Technique 175 

My = -y + u (50) 

where /j is the filter time-constant. In the simulation fi = 0.025 is selected. In the transition 
period the frequency of r leq may be higher than the edge-frequency of the low-pass filter, 

see the discussion in Section 2.3.4. To solve this problem, the time constant of the low-pass 
filter is made time varying: 

f(0.025/0.5)/, 0<f<0.5 
ju(t) = (51) 

[ 0.025, t>0.5 v ; 

Now the time constant of the low-pass filter is linearly increased from zero to 0.025s in half 

second and remains constant thereafter. 

For the singular perturbation approach described in Section 3, the simple form Tt = -D T i is 

used for the fast dynamics, where matrix D T is selected as 



A. 



0.001 
0.001 



(52) 



Besides the large parameter mismatches between the values in the plant model and the 

nominal values used in the controller given by equation (49), some disturbances are added 

to the plant model to test the robustness of proposed control algorithms: 

(a), the coefficients of viscous friction and coulomb friction in equation (46) are set as 

k vl = k v 2 =\0Nml(rad I s) and k c \ = k C 2 =5Nm, respectively. The generated friction terms are 

sufficient large with respect to gravitation forces, centrifugal and Coriolis forces in the plant 

model. 

(b). an additional disturbance torque during ~ 0.15s with constant amplitude of -lOOTVm 

is added to both robot joints to test the robustness of the control system in the transition 

period. 

5.3. Simulation results and discussion 

The simulation results of the trajectory tracking controller for rigid-body robots presented in 
Section 2 have been given in (Shi et al., 2008), where different sliding mode control 
approaches under different system uncertainties are compared. In this section, we discuss 
only the simulation results for flexible joint robots, which are illustrated by Figure 2 through 
Figure 5. 
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Fig. 2. Pure integral sliding mode control. Left plots: designed and real error dynamics of the 
link position tracking control (dotted-line: designed, solid-line: real, they are too close to be 
distinguished); middle plots: joint torque; right plots: required motor torque. 
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Fig. 3. Integral sliding mode based perturbation estimation approach with constant low-pass 
filter to extract the equivalent control. Left plots: designed and real error dynamics of the 
link position tracking control (dotted-line: designed, solid-line: real); middle plots: joint 
torque; right plots: required motor torque. 
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Fig. 4. Integral sliding mode based perturbation estimation approach with time varying low- 
pass filter to extract the equivalent control. Left plots: designed and real error dynamics of 
the link position tracking control (dotted-line: designed / solid-line: real, they are too close to 
be distinguished); middle plots: joint torque; right plots: required motor torque. 



position tracking error of jointl 



position tracking error of jointl 



position tracking error of jointl 




\,S,?*«««%|j,, 



time (s) 
position tracking error of joint2 



time (s) 
position tracking error of joint2 



time (s) 
position tracking error of joint2 




time (s) 



Fig. 5. Designed and real error dynamics of the link position tracking control (dotted-line: 
designed, solid-line: real) of the three control approaches, but without the singular 
perturbation treatment on the joint flexibility. Left plots: pure integral sliding mode control; 
middle plots: perturbation estimation approach with constant low-pass filter; right plots: 
perturbation estimation approach with time varying low-pass filter. 
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With Figure 2 the pure integral sliding mode control approach i.e. the controller given by 
equation (18) is demonstrated. For rigid-body robots, this controller has a prefect tracking 
control performance despite the large torque disturbance during the transition period (see 
(Shi et al., 2008)), but for flexible joint robots, the steady-state responses are not smooth 
enough, see both left plots in Figure 2. Similar to the case of rigid-body robots, there are high 
frequency oscillations in the joint torque and in the motor torque. The oscillation frequency 
for flexible joint robots is lower than the one for rigid-body robots because of the joint 
flexibility. For both types of robots this controller can not be used in practice due to the high 
level of chattering. 

In Figure 3, the simulation result of the controller given by equation (19) is presented, where 
the low-pass filter is the first order linear filter given by equation (50) with constant /j . As 
one can see from the middle and right plots of figure, the joint torque and the required 
motor torques are smoothed significantly due to the perturbation estimation solution 
(implying that this controller can be applied to real world robot systems). However, the 
performance of the position tracking control is decreased a little bit, see the both left plots of 
the figure. 

To recover the tracking control performance while keeping the joint torques and motor 
torques as smooth as possible, the time constant of the low-pass filter is made time varying 
according to equation (51). The simulation result is illustrated in Figure 4. Now, the position 
tracking control has a higher control accuracy, in both transition period and steady-state, see 
the both left plots of Figure 4. From the middle and right plots of Figure 4, one can see that 
the joint torques and the motor torques are still smooth enough, only in the transition period 
the frequency of these signals is higher than the case of Figure 3, because of the smaller time 
constant of the low-pass filter in this time range. Therefore, by tuning the time constant of 
the low-pass filter, the overall system performance can be improved. 

The control approaches demonstrated by the simulation results given by Figure 2 through 
Figure 4 are supported by the singular perturbation treatment on the joint flexibility. 
Without this treatment, none of the control approaches can work properly, see Figure 5 
(where all elements of matrix D T are set to zero). Therefore, joint torque signal as well as its 
time derivative is very important for the control of flexible joint robots. 

6. Conclusion 

The robust position tracking controller based on integral sliding mode for rigid-body 
manipulators is extended to the position tracking control of lightweight manipulators as 
well as flexible joint robots. Moreover, the control system takes the dynamics of joint motors 
into account. The joint flexibility is solved by singular perturbation approach which needs 
no parameter from the controlled system. Also, the current controller for the joint motors is 
a robust controller without involving the parameters of the electric motors and decoupling 
process. By using sliding mode PWM technique the current controller overcomes the 
disadvantages associated with the conventional build-in PWM in micro-processors or DSPs. 
For the link position tracking control only some rough nominal values are required. It is 
possible to achieve the pole-placement design without the exact knowledge about the 
manipulator system to be controlled. Moreover, the control design is mathematically easy 
and straightforward without involving the properties of the robot dynamics. The resulting 
control algorithms are simple enough for real-time implementation. The tradeoff between 
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chattering reduction and system robustness can be adjusted by the time constant of a low- 
pass filter. As the chattering level being significantly reduced, the control algorithms are 
applicable to real-life systems. Comparative simulation studies have confirmed the 
effectiveness of proposed control approaches and showed the potential toward the control 
of lightweight manipulators for high performance applications. Moreover, the presented 
design methodology can also be applied to other non-linear multi- variable dynamic systems. 
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1. Introduction 

Robots are important for industrial automation. Similar to CNC machining, robotic systems 
can be applied to numerous applications such as material assembling, welding, painting, 
manufacturing and so on. For control of robot manipulators, a conventional way is to 
establish their mathematical models in the joint space and therefore precise positioning of 
end-effector relies on control performance in the joint space. 

In terms of utilizations of industrial robots, it is well known that the position of end-effector 
is an important factor and significantly dominates the quality of final product. Based on 
given trajectories in work space (also known as Cartesian space), there are two main 
approaches for manipulator motion control, 1) one is to determine the desired joint space 
positions by solving the inverse kinematics, 2) the other is to consider the dynamic model in 
the work space directly (Feng & Palaniswami, 1993). In both approaches, tracking 
performance should be good enough in order to follow real time commands and achieve 
prescribed contours. In respect of the joint space approach, for instance, providing the 
tracking errors of each joint position can not be eliminated well, the end-effector can not 
track the desired profile precisely; to put it another way, once good tracking capability in 
any one of robot arms can not be guaranteed, the synchronization control task fails and 
thereby gives rise to unsatisfactory machining result. Consequently, it is worthy to 
investigate a better control strategy to guarantee the machining quality even in the presence 
tolerable tracking errors. To achieve this goal, we have to deviate the topic regarding control 
of robotic systems for a moment and address a certain core issues about contouring control 
systems. 

1.1 Definition of contour error 

First of all, a machining quality index called contour error is introduced. As shown in Fig. 1, 
the contouring error s corresponding to point P(t) is defined geometrically as the shortest 

distance from P(t) to the desired contour D and can be written as s =minP*-P(t) , 

v ' P D N N 

where P(t)eS is the actual position of end-effector at time t and P s D . Once the shortest 
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target P* is found, (ideal) resultant control effort will force P(t) moving towards P* firstly 

and then keep it attaching on the prescribed path. This control behavior involves two stages 
i.e., normal and tangential errors reduction. This concept is clear and has been widely used 
in contouring control designs (Koren, 1980; Chin & Lin, 1997; Ho et al., 1998; Yeh & Hsu, 
1999; Chiu & Tomizuka, 2001, Shih et al., 2002; Wang & Zhang, 2004; Hsieh et al., 2006; Peng 
& Chen, 2007a; Peng & Chen, 2007b; Chen & Lin, 2008). 
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Fig. 1. Definition of contour error. 
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Fig. 2. Special issues appeared in contour following control systems. 



1.2 Tracking and contouring control 

Based on the definition of contour error, the following is to illustrate a main discrepancy 
between tracking control and contouring control. Consider Fig. 2, suppose that A is the 
location of the end-effector and D is the corresponding command position at a certain time 
instant. Providing the tracking error 6a exists significantly, a resultant tracking control force, 
where the orientation is towards from A to D, is going to be generated by controllers such 
that an undesirable working path away from the desired profile is induced. Moreover, Fig. 2 
also reveals that a good tracking performance is just a sufficient, but not necessary, 
condition to reach good contouring performance. For example, the tracking error at point A 
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is apparently larger than the one at point B, but from contouring control point of view, 
contouring error is defined as the shortest distance from the end-effector to the desired path, 
so the position A is preferable in the contouring process rather than B. A couple of features 
and problems of contour control systems can be found in the tutorial study by Ramesh et al 
(2005). 

1.3 Contouring control strategies 

In terms of the literature on contouring control strategies, a well known cross-coupling 
control (CCC) structure (Koren, 1980) has been widely applied. Using this framework, 
contouring error attenuation can be carried out by designing various control strategies. For 
example, to raise machining speed, a modified CCC structure, where an additional pre- 
compensated controller is involved, called cross-coupled pre-compensation method (CCPM) 
(Chin & Lin, 1997) was proposed. However, these control components, namely tracking 
controller, contouring controller and feed-forward controller, are usually designed 
independently such that the coupling effects between them are not manifest. It is difficult to 
distinguish which one dominates the eventual contouring result. Thus, a systematic design 
procedure for CCC structure is highly desirable. To this end, transfer function methods for 
CCC structure are proposed (Yeh & Hsu, 1999; Shih et al., 2002; Zhong et al., 2002). These 
methods make the CCC design into a unit feedback control problem and offer a systematic 
analysis for stability and performance of a linear contouring system. However, superior 
tracking level is still needed to confirm final contouring qualities when these approaches are 
utilized. 

Some contouring control architectures, which involve coordinate transformation techniques, 
such as tangent-normal (T-N) coordinate frame (Ho et al., 1998; Chiu & Tomizuka, 2001; 
Wang & Zhang, 2004; Hsieh et al., 2006) and polar coordinate frame (Chen et al., 2002) have 
been presented in recent years. For T-N coordinate transformation approaches, contouring 
dynamics are decomposed into tangential and normal directions, where the tangential 
dynamics is associated with feed-rates while the normal dynamics is relevant to contouring 
error. Nevertheless, only the contour error of straight paths can be evaluated exactly. The 
normal errors just stand for approximated contour errors for arbitrary non-linear curves. On 
the other hand for polar coordinate transformation scheme, dynamics of radial orientation is 
derived in consideration of both circular and noncircular profiles. The polar coordinate 
based contouring control framework is adequate for circular profiles. For non-circular 
profiles, the precision of contouring error estimation relies upon that the radius variation 
with respect to angle change is small. However, for a given straight line which is (almost) 
perpendicular to x-axis may not satisfy this assumption. 

For the preceding coordinate transformation based contouring control schemes, the main 
advantage is that the goal of contouring controller design becomes clear and simple; in other 
words, a tangential controller focuses on maintaining a desired feed-rate while a 
normal/ radial controller is applied to eliminate normal/ radial errors. However, good 
contouring performance still depends on good tracking results when applying the T-N 
coordinate transformation methods. Once a large tracking error occurs, the contouring 
quality will be degraded considerably. 

In order to complete contouring tasks efficiently and accurately, an adequate control 
strategy is prerequisite. Computed torque method, which uses the nominal dynamic model 
of the robots to decouple and to linearize the nonlinear system, is one of the well-known 



186 Advances in Robot Manipulators 

approaches. However, when utilizing the computed torque method only, the resultant 
performance may not be satisfactory due to the effects caused by lump perturbations 
including uncertain parameters, modeling errors, load variations and nonlinear friction 
effects. For the sake of system robustness enhancement, several robust control theories such 
as learning control (Sun et al., 2006), H-infinity (Fang & Chen, 2002), sliding mode control 
(SMC) (Zhu et al., 1992; Chen & Xu, 1999) and adaptive control (Slotine & Li, 1988; Dong & 
Kuhnert, 2005; Lee et al., 2005) have been proposed. In recent decades, SMC and adaptive 
algorithm have been widely used to control of robot systems. Known as robust and accurate, 
SMC is a good candidate when systems are interfered by model uncertainties and 
exogenous disturbances. On the other hand, adaptive algorithm, which possesses learning 
behavior, is capable of estimating uncertain parameters when the parameters are not 
precisely known. Therefore in this study, by considering well known backstepping design 
technique, an integral type SMC is designed together with an adaptation law such that the 
controlled robotic system is robust against lumped perturbations and an adequate switching 
gain used in sliding controller is determined systematically without try and error and 
tedious analysis. 

2. Main Concerned Issues and Contributions on Robot Contouring Control 
Systems 

For concerning control of robot manipulator, the main control objective is to control the 
motion trajectory of the end-effector following a prescribed contour. For conventional 
trajectory control, a given profile in work space is decoded into independent reference joint 
positions and the success in contouring control task depends on tracking capability of 
individual robot joint. However, as argued in the preceding section, once one of the robot 
joint does not perform good tracking result, the end-effector may deviate from the desired 
path seriously. Therefore, the contouring control problem on a multi-link robot manipulator 
leads to a synchronization task of joint position, which can also be referred to as master- 
slave control scheme (Sarachik & Ragazzini, 1957). Moreover, it has been illustrated by a 
couple of experiments that good contouring quality doesn't necessary depend on good 
tracking performance (Peng & Chen, 2007a). Consequently for manipulator contouring 
control, how to relax tracking capability and preserve contouring precision becomes the 
main concerned issue in this chapter. 
To fulfill the main object mentioned above, some short-term tasks must be considered: 

(i) Define an equivalent contour error. 

(ii) Derive a contour error model for robotic system. 

(iii) Design robust contouring controllers. 

For task (i), it is well known that a closed-form solution of the exact contour error for an 
arbitrary curve is quite difficult to formulate. Thus, approximated contour error estimations 
are usually applied. Due to the use of inexact contour error models, attenuation of the 
approximated contour errors may not guarantee the elimination of real contour error 
especially in the presence of large tracking errors. As a result, an adequate equivalent error 
index should be defined. In this work, a new error variable named contour index (CI) is 
given by means of a geometric way in a virtual contouring space (VCS). The properties 
include: i) the CI is able to act as an equivalent contouring error and replace the normal 



Coordinate Transformation Based Contour Following Control for Robotic Systems 187 

tracking error (Hsieh et al., 2006, Chen & Lin, 2008) to be a new performance index without 
causing geometric over estimation and ii) the CI contributes to contour following behavior 
(Peng & Chen, 2007a). This phenomenon can be illustrated by referring Fig. 2, where the 
end-effector at A approaches to the real time command D through the desired profile 

instead of moving along AD directly. 

Regarding task (ii), the design processes are summarized as follows: dynamic correlation 

between joint space and work space is firstly established. The work space dynamics are 

further derived into the VCS, which consists of tangential and normal dynamics. Then, a 

dynamic equation of CI, which offers the end-effector to trail the prescribed path, is derived. 

Finally, based on the developed error dynamics, an adaptive sliding controller together with 

the idea of inverse dynamics control (Spong & Vidyasagar, 1989) is applied to provide 

system robustness against lumped uncertainties and fulfill the main control object. 

The advantage of the proposed method for manipulator contouring control problem 

contains: 

(1) The derivation of contour error model for robotic system is systematic 

(2) The closed-loop stability analysis is clear. 

(3) Final contouring quality can be maintained even if the end-effector doesn't 
track the real time command very well. 

(4) The proposed method is also extendable to three-dimensional case (Peng 
& Chen, 2007b). 

3. Proposed Contouring Control Framework for a Robot Manipulator 

3.1 Forward kinematics 

By using the Euler-Lagrange method, a dynamic equation of a vertical robot arm can be 
expressed as 

M(e)e+c(e,e)e+G(e)=T-F (i) 

where 8, 8 and 8e s .R n are the joint position, velocity and acceleration vectors, respectively. 
M(8)s s .R" xn is a positive definite inertia matrix; C(8, 8je 9?"*" is a matrix containing Coriolis 
and centrifugal terms; G(8)e!H" stands for gravitational term; TsiR" represents a torque 
vector and F e 91" is a disturbance vector referred to nonlinear friction effects. In this study, 
a two degree of freedom robot ( n = 2 ) is considered and depicted in Fig. 3. The position 
translation from joint space to work space can be calculated by considering forward 
kinematics as follows, 

Pa=h(e(t)) (2) 

Taking the twice time derivative of (2) gives 

p=je+je (3) 

where P a = [x y] T is the end-effector position vector in work space, J is the Jacobian 
matrix and T denotes the transpose. Note that the Jacobian matrix is assumed to be fully 



188 



Advances in Robot Manipulators 



known for position measurement and is nonsingular during the whole contouring control 
processes. The definition of matrices used in (2) and (3) are listed in Appendix. 




Fig. 3. Two degree of freedom robot. 

3.2 Contour generation 

Consider a desired profile in work space generated by 



= T„kP, +c„ 



(4) 



where P d =[x d y d ] T and P d =[x d y d ] T are position vectors in work space and VCS, 
respectively. As an example, a standard unit circle in VCS is made up of x d = sin(2;rft) and 
y d = cos(2/zft) , where the frequency f is relative to tangential velocity. T„ denotes a 
rotational matrix with a angle /? and the diagonal gain matrix k (please see Appendix) is 
relative to the length of major axis and minor axis of the desired profile. The term 
c o =[c xo c o ] T denotes a shifting operator. The geometric meaning of (4) is depicted in Fig. 

4, where an oblique ellipse can be generated from a unit circle by the matrix operation, T„k . 

. Y 




< P 



Fig. 4. Command generation via matrix operation. 
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3.3 Derivation of contour error dynamics 

Define the tracking error in work space as e = P d -P a =[e x e ] T and then the resulting 

error dynamics is 

e = P d -P (5) 

Subsequently, the error relationship between VCS and work space can be represented by 

E = T(T p K) 1 e (6) 

where e = [e n s t ] T denotes a transformed error vector including normal tracking error and 

tangential tracking error. The projection-matrix T is orthogonal, continuous and 
differentiable, defined by 



-sin/ cosy 
cos y sin y 



(7) 



where y is an instantaneous inclination of the tangential direction corresponding to the 
circular profile shown in Fig. 5. From the definition of tracking error in work space, Eq. (6) 
can be rewritten as 

E = T^k)^ -P. ) = T^-^]+t(t p k)- 1 c o (8) 

E 

where P a = (TpK) _1 P a and E = [E x E ] T . The corresponding geometrical meaning is shown 
in Fig. 5. Eq. (8) means that the information in the work space is transformed into the VCS 
through T(T„k) _1 ; namely, the vectors P a , P d and ||e|| in the work space are transformed to 

be P a , P d and |E| in VCS, respectively. Then the error vector ||E| can be further 

decomposed into tangential and normal directions through the matrix T . 

Fig. 5 shows that a moving T-N coordinate attaching to the profile guides P a to follow P d 

along the profile. In the control point of view, Eq. (8) also indicates that P d - P a = if and 

only if e = . Therefore, the control problem turns into a regulation problem in both 

tangential and normal directions. 

From (6)-(7), since T" 1 = T , it follows 

e + 2TTe + TT e = t(t p k) _1 e (9) 

Substituting (5) into (9) yields 

£ + 2TTe + TT e = T^k) -1 (p d - P a ) 

= T( TpK )- 1 p d -T(T f ,K)- 1 (je + je) (1Q) 

= t^k^p, - t^k)- 1 je - t^k)- 1 jfvr'fu - f - ce - g)] 

= 2 + f It 
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where X = t(t p k) _1 (p d - jt))+ r(c8 + g) , f E = Tf and r = T^k)" 1 JM" 1 . Eq. (10) represents 
the error dynamics in the VCS. 



desired profile in work space 




T„kT 



T(TpK)- 




Fig. 5. Robot contouring system. 

Consider the inverse dynamics compensation method (Spong & Vidyasagar, 1989), a 
nominal control torque x = [r 1 r 2 ] T for (10) is chosen as the form 



T=r^-T.) 



(ii) 
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where T and X are denoted as the nominal value of T and X (i.e., I" = I" + T , X = X + X ), 

respectively and t e =[r m T a ] J . 
Substituting (11) into (10) yields 



e + 2TTe + TTe = 2 + f E It 

= s+f E -f[f" 1 (i-T, 
= i-rT+f,+T, 

= A + T 



Tx 



(12) 



£ t 


+ 2 


' y 
-j 0_ 


e n 


- 


- 2 

■ 2 


s n 


= 


A t +r at 



where A = E rT + f E =[A n A, ] T represents model uncertainty and external disturbance 
(for example, the friction effects). Refer to (9), it can be rewritten as 



(13) 



Providing the desired feed-rate is set to be constant for predetermined profiles, it follows 
y = 0. 

From Fig. 5, one can find that the mismatched term S n in normal direction is caused by the 
existence of s t . Thus, the elimination of normal tracking error e n causes an over-cutting 
segment S n when e t ^ . In order to solve this problem, the CI is introduced in the VCS 
and is defined by 



= *„-£. 



(14) 



where S n = R - (R 2 -sf) 1/2 . Note that the CI replaces s n to be a new performance index or 
to be regarded as an equivalent contouring error during control process; to put it simply, the 
purpose of the contouring control is to eliminate s [nd instead of s n while e t * . Invoking 
(13) with (14) yields 



where 



^Ind = *"«, + >h +1l+1i+ A n (I 5 ) 

'7, =-2^, + r 2 ^„ -rs,-{e t e t f -a^ n -\s\ +e t (2ys n +/£» +y 2 s t )]-a- 1/2 

1/2 

-£ t A tCT - 1/2 
cr = R 2 -£ t 2 



Examining (14) again, it is necessary to remind that for all the control period, the condition 
|f t |<R must be guaranteed. Consequently, the maximum allowable tracking error in 

tangential direction is R , which is not a crucial condition in practice. 
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Therefore, the modified system dynamics in VCS becomes 

s\ - 2/£ n - y 2 £ t = A, + r a (16.a) 

^i„d='/ 1 +'/2+'/ 3 +A 11 +z- m (16.b) 

For (16), contouring controller design will be addressed in the next section. 

4. Contouring Controller Design 

In this section, design of contouring controller is considered separately for tangential and 
modified normal dynamics. For demonstration purpose, a general proportional-derivative 
(PD) controller is applied in tangential error equation. It is well known that the PD 
controller is capable of achieving stabilization and improving transient response, but is not 
adequate for error elimination. Consequently, tangential tracking errors exist unavoidably. 
Under this circumstance, we are going to show that precise contouring performance can still 
be achieved by applying the CI approach. Building on the developed contouring control 
framework, the tangential and normal control objects can be respectively interpreted as 
stabilization and regulation problems. 

4.1 Design of tangential control effort 

Considering the tangential dynamics of (16.a), a PD controller with the form 

^ = -KvfJ, - K pi^, -2/4 - r 2 £ t (17) 

is applied. Substituting (17) into (16. a) results in 

e\+K vt £ t +K n £ t =A t (18) 

where K vt and K pt are positive real. The selection of control gains should guarantee the 
criterion U, I < R . Eq. (18) indicates that the tangential tracking error cannot be eliminated 
very well due to the existence of A, . However, it will be shown that the existence of £ t 
causes no harm to contouring performance with the aid of CI. 

4.2 Design of normal control effort 

In the following, an integral type sliding controller for the modified normal dynamics is 
developed by using backstepping approach. Firstly, let £ lnd = £ lndl and define an internal 
state w . Then the system (16. b) can be represented as 



'Indl 



(19.a) 

*fedl = ^Ind2 (19-b) 

4,d2 = f?i + 1i + li + A „ + T m (19.c) 
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Assume that the system state s Indl can be treated as an independent input ^ (w) , and let 

^i„di=^=- k i w ( 20 ) 
where k 1 > . Then, consider as a Lyapunov function 

V 1= w 2 /2 (21) 
The derivative of (21) is 

V, =w^=-k lW 2 <0 (22) 

In practice, there may exist a difference between s lndl and 0, . Hence, define a new error 
variable by z 1 = e lndl - fc , which gives 

w = Zj +^j (23. a) 

and 

Z l=^I„d2-A ( 23 - b ) 

Second, in a similar manner, consider e Ind2 as a virtual control input and let 

^i„d2=&=- w - k 2 z i+A ( 24 ) 

Selecting as a Lyapunov candidate 

V 2 =w 2 /2 + z 2 /2 (25) 

and taking its time derivative gives 

V, =w{z 1 +0 1 )+Z 1 {£ [nd2 -0 1 ) 

= w(z 1 +0 1 ) + z 1 {0 2 -0 l ) (26) 

= -k t w 2 -k 2 z 2 <0 

Note that the criterion (26) is achieved only when the virtual control law (24) comes into 
effect. Taking the constraint into account, one can design a sliding surface as z 2 = s lnd2 -(f> 2 , 
and then the augmented system can be represented as 

vj =z 1 +tj> 1 (27.a) 
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z 1= z 2 + ^-(4 (27. b) 

z 2 = ^ + ;; 2 + rj, + A n + r in - ^, (27. c) 

Suppose that the parameter uncertainties and external disturbances satisfy the inequality 
max(|// 3 + A n |)< O., where Q is an unknown positive constant, then the final control object is 

to develop a controller that provides system robustness against CI . 
Design a control law in the following form 

f« =-1i -V, -Zi -k 3 z 2 +^ 2 -£sgn(z 2 ) (28) 

where £ > Q denotes the switching gain. Select a Lyapunov candidate as 

V 3 =w 2 /2 + z 2 /2 + z 2 /2 (29) 

From (28) and (29), one can obtain 

V 3 < w(zj + ^ )+z 1 (z 2 +fa-fa )+ z, (^ + rj 2 - (i 2 + r m )+ fi|z 2 | 

< -k x w 2 -k 2 z 2 -k 3 z 2 -z 2 £sgn(z 2 )+Q|z 2 | (30) 

= -2kV 3 -|z 2 |(£-fi)<-2kV 3 

where k t =k 2 =k 3 =k is applied. Therefore, system (27) is exponentially stable by using the 
control law (28) when the selected £ satisfies £, > Q, . Since the upper bound of Q may not 
be efficiently determined, the following well known adaptation law (Yoo & Chung, 1992), 
which dedicates to estimate an adequate constant value £ a , is applied 

| a =Yz 2 sat(z 2 ,p) (31) 

where £ a is denoted as an estimated switching gain and 

f v P>0 \z,\>p 

, (32) 

[¥=0 \z 2 \<p K ' 

stands for an adaptation gain, where the use of dead-zone is needed due to the face that the 
ideal sliding does not occur in practical applications. For chattering avoidance, the 
discontinuous controller (28) is replaced by 

T ,-u =-'/l-'/2- Z l- k 3 Z 2+<4-£, Sat ( Z 2'P) ( 33 ) 

The saturation function is described as follows 
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sat(z 2 ,p):= 



sgn(z 2 ) \z 2 \>P 



(34) 



where p is relative to the thickness of the boundary layer. 

Let the estimative error be £ a , i.e., f a = £ a - £ a and then select a Lyapunov function as 



V 4 =w 2 /2 + zJ/2 + z^/2 + |;, 2 /2^ 



(35) 



The time derivative of (35) is 



V 4 =WW + ZjZj + z 2 z 2 +| a f a /f / 

<-k a w 2 -k,z 2 -k 3 z 2 -(z 2 £ a sat(z 2 ,p)-Q|z 2 |j-£ a z 2 sat(z 2 ,p) 
= -2kV 4 -(z 2 £,sat(z 2 ,p)-Q|z 2 |) 
= -2kV 4 -|z 2 |(^|z 2 |/p-n) 
= -2kV 4 +«9(t) 



(36) 



where 5(t) is bounded by i9(t) < i9 max . In general, £ a > Q. is available. Suppose that £, = Q. , 
it follows 5(t) = -n|z 2 |(|z,|/p-l). The maximum value 3 max =pQ/4 occurs at \z 2 \ = p/2 . 
Eq. (36) reveals that for t — > °° , it follows 



V 4 (t) < exp(-2kt)V 4 (0)+ f exp(-2k(t-y))9(u)dw 

Jo 

< exp(- 2kt)V 4 (0) + *£ [1 - exp(- 2kt)] < ^ 




so 
o 

S en 

■P U 

~> 
"o c 
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U 
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£ t 
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Fig. 6. Block diagram of the proposed contouring control scheme for a 2-Link robotic system. 
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By (37), the system is exponentially stable with a guaranteed performance associated with 
the size of control parameters p and k . The overall contouring control architecture is 
illustrated in Fig. 6. It is similar with the standard feedback control loop, where the main 
control components are highlighted in the dotted blocks. 

Remark. 1 For illustration purpose, a PD controller is applied to the tangential dynamics 
such that tangential tracking error cannot be eliminated completely. Of course one can also 
apply a robust controller to pursue its performance if necessary. However, the following 
simulations are going to show that even in the presence of tracking errors, the contouring 
performance will not be degraded by using the proposed contouring control framework. 
Remark. 2 The action of the adaptive law activates when IzJ > p . It means that for a given 

small gain value, £ a will be renewed in real time until the criterion |z 2 | < p is achieved. 



5. Numerical Simulations 

In this section, a robot system in consideration of nonlinear friction effects is taken as an 
example. The friction model used in numerical simulations is given by 



isgnfek +(F sl -Fjexpf-fe /0 sl J 



(38) 



where F ci is the Coulomb friction and F si is the static friction force. 6 si denotes an angular 
velocity relative to the Stribeck effect and o" si denotes the viscous coefficient. The suffix 
i = 1,2 indicates the number of robot joint. 
The parameters used in friction model are: 

F cl = 0.025 , F c2 = 0.02 ,F„ = 0.04 , F s2 = 0.035 
el = 6> 2 =0.001, er 8l =0.005 and <y s2 =0.004. 

According to the foregoing analysis, an adequate switching gain is suggested to be 
determined in advance for confirming system robustness. Thus, estimations are performed 
previously for two contouring control tasks, i.e., circular and elliptical contours. Fig. 7(a) 

and (c) show the responses of sliding surface and Fig. 7(b) and (d) depict the response of £ a 

during update. 

From Fig. 7, it implies that the sliding surfaces were suppressed to the prescribed boundary 

layer by integrating with the adaptation law. The initial guess-value £ a (0) = 12 and the 
adaptation gain ¥ =100 are applied in (31). According to (32), an adequate value of £ a was 
determined when |z 2 | < p = 0.0025 is achieved. From the adaptation results shown in Fig. 

7(b) and 7(d), the conservative switching gains £ a = 18 and 16.5 are adopted to handle 
circular and elliptical profiles, respectively. 
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Fig. 7. Responses of sliding surface and estimated robust gain, (a)-(b) for circular contour, 
(c)-(d) for elliptical contour. 

5.1 Circular contour 

The following values are used for the control of circular profile: 

= 0, k x =k y =0.1, f = 1, (c xo ,c vo )= (0.21, 0.15) 
k = 10, K vt = 9, K Pt =20, m, =7.8, m 2 =0.37 



Exact system parameters of two-arm robot are 

m, = 8.344 , m 2 = 0.348 , 1, = 0.25 , 1 2 = 0.21 

In this case, the position of starting point in the working space is set to be at 
P a (0) = [ x o YoY =[0.21 0.25] T . For a given position P,(0), initial joint positions can be 
calculated by applying the inverse kinematics as follows 
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2 (O)= 



0,(0) = tan" 1 



■yt-il-il 



21J 2 



tan 1 



1 2 sin# 2 
1. +1, cost 



(39) 



Referring to the simulations, Fig. 8 obviously illustrates the contour following behavior. It 
shows that even though the real time command position (i.e., the moving ring) goes ahead 
the end-effector, the end-effector still follows to the desired contour without significant 
deviation. The tracking responses are shown in Fig. 9(a) and (b), where the tracking errors 
exist significantly, but the contouring performance, evaluated by the contour index e Ind , 

remains in a good level. The corresponding control efforts of each robot joint are drawn in 
Fig.lO(a)-(b). Examining Fig. 8(a) and (b) again, it can be seen that the time instants where 
the relative large tracking errors occur are also the time instants the relative large CIs are 
induced. The reason can refer to the dynamics of CI given in (16). Due to the face that (16b) 
is perturbed by the coupling uncertain terms ;; 3 when s t * , the control performance will 
be (relatively) degraded when large tangential tracking errors occur. 



O Command position 




O Command position 
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x-axis(m) 

(a) 
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Fig. 8. Behavior of path following by the proposed method. 
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Fig. 9. Performance of tracking and equivalent contouring errors. 
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Fig. 10. Applied control torque. 
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5.2 Elliptical contour 

In this case, the value of parameters are the same with those used in the previous case 
except p = n /3 , k x = 0.15 and k = 0.1 . 

Fig. 11 evidently demonstrates that the proposed method confines the motion of the end- 
effector to the desired contour. The tracking performance and contouring performance are 
shown in Fig. 12(a) and (b), respectively. The manipulator motion remains attaching on the 
elliptical profile even though the end-effector doesn't track the real time command precisely. 
The result is consistent with the behavior illustrated in Fig. 2, i.e., the end-effector at A 
approaches to the real time command D through the desired path without causing short 
cutting phenomenon. Moreover, it has been demonstrated that the CI approach is also 
capable of avoiding over-cutting phenomenon, which is induced by the T-N coordinate 
transformation approach, in the presence of large tracking errors (Peng & Chen, 2007a). The 
simulation results confirm again that good contouring control performance does not 
necessarily rely on the good tracking level. The corresponding continuous control efforts of 
joint-1 and -2 are shown in Fig. 12(a) and (b), respectively. 
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Fig. 11. Partial behavior of path following by the proposed method. 
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Fig. 12. Performance of tracking and contouring. 
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Fig. 13. Applied control torque. 
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6. Conclusion 

In the robotic motion control field, positioning and tracking are considered as the main 
control tasks. In this Chapter, we have addressed a specific motion control topic, termed as 
contouring control. The core concept of the contouring control is different from the main 
object of the tracking control according to its goal. 

For tracking control, the desired goal is to track the real time reference command as precise 
as possible. On the other hand, the main object is to achieve precise motion along prescribed 
contours for contouring control. Under this circumstance, tracking error is no longer a 
necessary performance index requiring to be minimized. To enhance resulting contour 
precision without relying on tracking performance, a contour following control strategy for 
robot manipulators is presented. 

Different from the conventional manipulator motion control, a contour error dynamics is 
derived via coordinate transformation and an equivalent error called CI is introduced in 
VCS to evaluate contouring control performance. The contouring control task in the VCS 
turns into a stabilizing problem in tangential dynamics and a regulation problem in 
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modified normal dynamics. The main advantage of the control scheme is that the final 
contouring accuracy will not be degraded even if the tracking performance of the robot 
manipulator is not good enough; that is, the existence of tracking errors will not make harm 
to the final contouring quality. This advantage has been apparently clarified through 
numerical study. 
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Appendix 

The matrices used in this paper for 2-link rigid robot are listed in the following 



M = 



M„ M,, , 

M„ M ' 



2m 2 l 1 l c2 <9 2 sin(# 2 ) -m 2 l 1 l c2 #j sin(# 2 ) 
m 2 \ 1 \ c2 1 sin(# 2 ) 

mjgl^ cos^^ + rrijgflj cos(^ 1 ) + l c2 cos(^ 1 + 0. 
m 2 gl c2 cos(6 l 1 +0 2 ) 
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where 



M u =m 1 I* 1 + m 2 (lj +lj; 2 +21 1 l c2 cos(# 2 ))+Ii +I 2 
M 12 = m 2 (l 1 l ( , ; , cos(^ 2 )+lJ 2 )+I 2 
M 21 = m 2 (l 1 l c2 cos(# 2 )+l* 2 )+I 2 



M„ 



= m 2 l^ 2 +I 2 



I -Al I. =J_ m .l 2 
2 12 



The vector used in (2) is 



h(e(t))= 

and the Jacobin matrix is defined as 



lj cos(> 
L sin( 



j+ljCOS^j +( 

)+Lsin(A +l 



J 



-liSin^J-^sin^ + 6> 2 ) -I 2 sin(#, + )" 
I, cos(6' 1 ) + l 2 cos(6' 1 +6 2 ) 1, cos(6 l 1 + 6 ) 
Regarding command generation, the operation matrices are 



cos/? -sin/? 
sin/? cos/? 



, k = 



k % 
sr„ 
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1. Introduction 

The present chapter is aimed at systematically exposing the reader to certain modern trends 
in designing advanced robot controllers. More specifically, it focuses on a new and 
improved method for building suitable adaptive controllers guaranteeing asymptotic 
stability. It covers the complete design cycle, while providing detailed insight into most 
critical design issues of the different building blocks. In this sense, it takes a more global 
design perspective in jointly examining the design space at control level as well as at the 
architectural level. 

The primary purpose is to provide insight and intuition into adaptive controllers based on 
Christoffel symbols of first kind for a serial-link robot arm, (Mulero-Martinez, 2007a). These 
controllers are referred to as static since the positional dependence of the nonlinear 
functions. In this context, the preferred method of nonlinear compensation is the method of 
building emulators. Often, however, the full power of the method is overlooked, and very 
few works deal with these techniques at the level of detail that the subject deserves. As a 
result, the chapter fills that gap and includes the type of information required to help control 
engineers to apply the method to robot manipulators. Developed in this chapter are several 
deep connections between dynamics analysis and implementation emphasizing the 
powerful adaptive methods that emerge when separate techniques from each area are 
properly assembled in a larger context. 

After beginning with a comprehensive presentation of the fundamentals of these techniques, 
the chapter addresses the problem of factorization of the Coriolis/ centripetal matrix, 
(Mulero-Martinez, 2009). This aspect is crucial when designing non-linear compensators by 
emulation. At this point, it is provided a concise and didactically structured description of 
the design of emulators as matters stand, (Mulero-Martinez, 2006). Specifically, emulators 
are split up into sub-emulators to improve and simplify the design of controllers while 
making faster the updating of parameters. From a practical point of view, the 
implementation is developed by resorting to parametric structures. This means to obtain a 
set of system's own function as regression functions. 

Most of the adaptive schemes start from the notable property of linearity in the parameters, 
which lead naturally to equivalent structures when designing emulators for the nonlinear 
terms. When the linearity in the parameters (LIP) is considered as a first assumption in the 
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development of adaptive schemes, it is clear that there exists a strong connection to the LIP 
emulators formulated in terms of a regression matrix and a vector of parameters. The main 
difference between standard adaptive schemes and the proposed approach stems from the 
idea of developing efficient controllers. The present work is aimed by attempts to mitigate 
the "curse of dimensionality" by exploiting the representation properties associated with the 
matrix of Coriolis/ centripetal effects. By recalling the connection between LIP 
representation of robot manipulators and LIP adaptive emulators, it can be asserted that 
standard scheme matches perfectly with a dynamic emulator. Thus, the regression matrix, 
depend not only on the position joint variables but also on the velocity and acceleration 
variables. 

As regards to the control, a novel theorem guarantees the stability for the whole system and 
is based on the Lyapunov energy. The proof is generalized to cope with a realistic case 
where both a functional reconstruction error and an external disturbance are present. It 
should be observed that the functional reconstruction error is caused by not using a number 
of regression functions appropiately distributed in the space. As a result, these 
considerations lead to a quite different approach, since it is required to analyze the initial 
conditions of the errors to guarantee the validity of the approximation. The specification of a 
range of validity causes that the stability holds only inside a compact set. As a consequence, 
the proof guarantees semi-global stability as opposed to the standard schemes where the 
stability is attained in the whole state space, in a global sense. Apart from these 
considerations, a number of remarks have been made to address some special aspects such 
as the boundedness of the parameters, the ultimately uniformly boundedness of all the 
signals and the stability in the ideal case. 

The main benefit of the proposed controller is that it allows to derive tuning laws only for 
inertia, gravitational and frictional parameters. The Coriolis parameters are not necessary to 
be used because of the approximation based on Christoffel symbols. This is very useful to 
implement adaptive controllers since the number of nodes diminishes and the 
computational performance improves. Previously, an extensive analysis of the mechanical 
properties for a robot has been discussed. The regression functions for the adaptive 
controller depend on the non-linear functions associated with the inertia matrix, and 
therefore, a discretization of positions could be done for the inertia matrix. This is a very 
useful aspect because the position space for a revolute robot is compact and in consequence, 
the number of nodes is limited to approximate a non-linear function. 

The plan of the chapter is as follows. In section 2 the representation properties for the 
Coriolis/centripetal matrix are analysed. An interpretation for the Coriolis/ centripetal 
matrix is presented and the description by means of the Christoffel symbols of first kind and 
fundamental matrices are provided. In section 3, emulators are used to approximate the 
non-linearities of a robot using the properties presented in the previous section and the 
Kronecker product. The next section presents the design of the adaptive controller in terms 
of a control law and a parameter updating law. This section concludes with a theorem that 
guarantees the stability for the whole system and is based on the Lyapunov energy. Finally 
an example of a 2-dof robot arm is used to illustrate the theorem. 

2. Representation of the Coriolis/Centripetal Matrix. Fundamental Matrices 

In this section some notions regarding the representation of the Coriolis/centripetal 
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matrices are introduced. All the ideas presented here constitute an original contribution and 
have many interesting implications in the field of robotics. To this end, fundamental 
matrices are introduced and described in terms of their structure. Moreover, some emerging 
properties are analyzed, allowing one to build the Coriolis/centripetal matrix in a simple 
way. Let start with the definition of the matrix Mr> which from now on will be called the 

inertia derivative matrix. 



Definition 1: 



M D (q,x) = £— ^xej 
j=i "4j 



(1) 



where M ( q ) is a generalized inertia matrix of dimension n x n a unitary vector of 
dimension n with a value 1 in the position j and x is an arbitrary vector of dimension n . 
It is noted that if x represents the generalized velocity vector, the matrix M D will stand for 
the gradient of the generalized momentum with respect to the position coordinates q . This 
means that the gradient of the kinetic energy as a quadratic form ^x T M(q)x relative to the 

joint position can be written as |Mj(q,x)x . Another representation of M D is showed 

below. 

Property 1: The matrix Mo ( q, x ) can be expressed as 



M D (q,x) = ^^-Xi 



(2) 



Proof: It is very easy to show that c g xej — YA=i ! jr Lx i e J since the following intermediate 
equation is obtained 



9M(q) 

9q ( 



^8U: T 

/ e\ 



T V^ 0M: J 

i = / ^ X i e i 



(3) 



Using the definition 1 and the identity (3) the hypothesis of the property is concluded 

M n (q,x)=> > L X:e; => L x, (4) 

Q.E.D. 

Now a new matrix will be introduced and from now on will be called as inertia velocity 

matrix, playing a central role in the representation theory. 

Definition 2: Let define M v (q,x ) in the following way 



M v (q,x) 



0Mi <9M n 
-x,..., x 



[ 9q 



dq 



: > L xe i 



(5) 
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The inertia velocity matrix M v (q,x) receives its name from the fact that when x = q , the 
term M v (q,q) will be the time differentiation of the generalized inertia matrix, i.e. M(q) . 
The following property provides an alternative way to write the matrix M v . 
Property 2: The inertia velocity matrix can be also expressed as 

Proof: It is known that -^p-xe; =Z]jLi"fl —Lx i e i arK ^ us i n g the definition 2 the property is 
proved as follows 



i=ij=i C4j j=1 









Q.E.D. 



2.1 Properties of the fundamental matrices 

Subsequently, some properties related to the fundamental matrices are analyzed. Following 
a systematic methodology, the properties have been classified into two groups: 
commutation properties and representation properties. 

2.1.1. Commutation properties 

Commutation properties permit interchange of an external arbitrary vector y and a vector 

x passed to a fundamental matrix as an argument. The following property makes possible 
the commutation while keeping the type of the fundamental matrices. This means that the 
transpose of the inertia derivative matrix can be transformed into the same structure by 
simply interchanging the roles of x and y . 

Property 3: Mj(q,x)y = Mp(q,y)x 

The proof of the last property follows directly from the definition of M D . The following 
property allows to pass from a type of fundamental matrix to another commuting the 
vectors x and y . 

Property 4: M v (q,x)y = M D (q,y)x 
Proof : 

M v (q,x)y = ZU^xeJy = EU^Yi = M D (q,y)x (8) 

Q.E.D. 

2.1.2. Properties of representation of the Coriolis/centripetal matrix 

These properties are very important to describe the Coriolis/centripetal matrix from the 
fundamental structures. 
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Property 5: 

M T D (q,q) = S^^qe, T (9) 

Proof: First of all, the transpose of the inertia derivative matrix can be represented as 
Mj,(q,q) = Zr=i e i a i T Zr=iir^ e i r usm g the definition 1 and the fact that the differentiation of 

the inertia matrix with respect to the generalized coordinate q ( is a = Z^iT^ei 1 ■ Since 

scalar product is commutative the following expression is derived, q T -j- L = -g- L q , and as a 

result Mj, (q,q) = TZ s i&illt~i~&r~A e: I ■ The order of summation is needed to be permutated to 
get the proposed identity. 



f 8M ^ 



T 



V 



aq, 



h dMl 



™IM = YL^\ qei r =E^qe| r (10) 



oq 



Q.E.D. 

Below, some representations of the Coriolis/ centripetal matrix are introduced as properties 

derived from M D and M v . 

Property 6: The matrix of Coriolis/ centripetal effects can be expressed as 

C(q,q) = ifM v (q,q) + M D (q,q)-M T D (q,q)) = i(M v (q,q) + j(q,q)) (11) 

where J(q,q) = M D (q,q)-Mj,(q,q) is a skew symmetric matrix, i.e. J(q,q) = -J T (q,q) . 
Proof: This is an immediate consequence of the representation of C(q,q) by means of the 
property 1 and the fact that the inertia velocity matrix is M v (q, q) = M (q) . 
In a general way, the following representation can be derived 

C(q,x) = i(M v (q,x) + j(q,x)) (12) 

wherexis a vector of dimension n and J(q,x) = M D (q,x)-Mj(q,x) is a skew symmetric 

matrix. It is remarkable that the definition of the Coriolis/centripetal matrix presented 
above, is different from the definition given by (Wen,1988) in the identity 2, 
C(q,z)z = -j(M D (q,z)-J(q,z)zj . An interesting property which is a direct implication of 

the property 4 is that, by setting x = yin C(q,x)y . 

Property 7: The Coriolis/centripetal force can be represented as 

C(q,q)q = ^M(q)-|M T D (q,q)jq (13) 

or in a general form as 
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C(q,x)x = M v (q,x)x-iM£(q,x)x (14) 

where x is an arbitrary vector of dimension n : 

Property 8: The Coriolis/ centripetal matrix commutes with external vectors 

C(q,x)y = C(q /y )x (15) 

Proof: In order to see this point the representation of the Coriolis/ centripetal matrix will be 
used as a sum of the inertia velocity matrix, M v (q,x)and the skew symmetric matrix 

J(q,x) given by equation (12). 

C(q,x)y = |(M v (q,x)y + T(q,x)y) (16) 

On one hand, it is known that j(q,x) = M D (q,x) — Mp(q,x) .Using the commutation 
properties 2 and 3 the following expression is derived 

C(q,x)y = !(M D (q,y)x-MS(q,y)x + M D (q,x)y) (17) 

On other hand, J(q,y)x = M D (q,y)x — M D (q,y)x and then applying the commutation 
property 3 to M D (q,x)y the following result is achieved as claimed 

C(q,x)y = i(j(q,y)x + M v (q,y)x) = C(q,y)x (18) 

Q.E.D 

3. Design of Emulators for Robot Manipulators. 

3.1 Functional and Linear Parameterization. 

The approach that follows is founded on the idea to find an emulator as a function close to 
the non-linear terms involved in the dynamics equations of a robot manipulator. In order to 
get a model from a practical point of view, uncertainties in the nonlinear terms, getting arise 
from the partial information about the exact structure of the dynamics, must be taken into 
account. The inaccuracies of a model can be classified into two classes: structured and 
unstructured uncertainties. The first kind of uncertainties comes out from the inaccuracies of 
the parameters whereas the unstructured uncertainties are related to unmodeled dynamics, 
see (Slotine & Li,1991). Thus, the uncertainties can be adaptatively compensated by defining 
each coefficient as a separate parameter so that the dynamics can be expressed in the linear 
in the parameters (LIP) and this means that nonlinearities can be split up into an unknown 
vector of physical parameters P and a known matrix of basis nonlinear functions 
I'^q^y) comprising the elements of M(q), C(q,q), G(q) and F(q,q) , referred to as 
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regression matrix. Therefore, the nonlinear function f (x) can be written in this sense adding 
a term of error e , see (Ge et al., 1998). 

i(q,q,x,y) = W(q,q,x,y)F + t (19) 

The linearity of the parameters is the major structural property of robot manipulators and 
has been analyzed in (Lewis et al., 2003). This linear factorization is always possible to be 
done for the rigid body dynamics of a fixed-based manipulator as long as the physical 
uncertainty is on the mass properties of the robot links. Furthermore, linearity of the 
parameters is the first assumption in the most of adaptive controllers. An alternative 
representation of the nonlinear component is as follows 

f(q,q,x,y) = R(q,q)v(x,y) (20) 

where R(q,q) = (M(q) C(q,q) G(q))e M nx(2n+1) and v = (y T x T l)eK 2n+1 . This 

factorization is always attainable whereas the linearity in the parameters (LIP) is only 
obtained under some circumstances. In the literature, emulators based on regression 
matrices have been used to approximate the nonlinear dynamics as a whole, as follows 

AA(q,q,q) = M(q)q + C(q,q)q + G(q) (21) 

As an attempt to obtain more efficient computation, the emulator approximating the 
nonlinearity f(x) is split up into several smaller components: 

f(q,q,x,y) = f m (q,y) + f c (q,q,x) + f g (q) + f f (q,q) (22) 

The function f m (q,x) = M(q)y , stands for the nonlinearity of inertial terms and can be 
written taking into account that the components of M(q) are continuous functions of their 

arguments so that each component can be uniformly approximated on any compact subset 
of the state space by an appropriately designed emulator. 

From now on we assume that the number of parameters to approximate the column i of a 
matrix is L . 



3.2 Inertia matrix M(q) . 

3.2.1 Ideal Case 

The inertia matrix M(q) consists of column vectors M ; (q) that can be approximated by 
regression matrices 

M i (q) = W mi (q)? mi (23) 
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where x ¥ m , (q) £ M^a.. (R) is a regression matrix of known robot functions and § m £ R ' is a 
vector of unknown parameters (e.g. masses, inertia moments and link parameters). Then, 
with an arbitrary vector x £ M n , there follows the decomposition 



M(q)x = 5> i (q)x i = ^^(q)^ 

i=l l i=l 



(24) 



On the assumption that the same regression matrix serves for each column, i.e. 

* mi (q)=* m; (q) = -\(q) = , Pm(q)6M,xl(K). °«e may rewrite equation (24) by 
resorting to the Kronecker product as 

M(q)x = O m (q,x)$ m (25) 

where § m £ R is an 1-dimensional vector of parameters and 

<E> m (q,x) = lx ® l J jr m (q))e A^px^K) is the regression matrix of the generalized inertia 
matrix. 



3.2.2 Real Case 

Given a closed, bounded subset QCR°, and a specified accuracy e m , there exist values 

for the design parameters 1; and S, m so that for all q(t)e£2 the following inequality is 
satisfied: 

|h(q)-1V(qK m J< £mi (26) 

For the sake of simplicity, we have assumed that W m (q) are equal for all i = l,...,n . The 
inertia matrix consists of column vectors that can be approximated as 



M(q) = $>, (q)ej = $> m , (q)? m e? 



(27) 



where e m (q) £ M nxn is the functional reconstruction error. 

It is convenient to underline that the vector function <t> m (q,x) only can be expressed by the 
Kronecker product whenever the hypothesis O m (q,x) = l J' m . (q) = l J' m (q) holds for all 
i = l,...,n.Asa consequence, a linear transformation with a change of basis can be derived 
in terms of the Kronecker product 

M(q)x = a> m (q,x)$ m +E m (q,x) (28) 
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The expansion given by (28) actually approximates the linear transformation M(q)x to a 
certain degree of accuracy E m (q,x) = £ m x by using an emulator with the position joint 

vector as the input. Specifically, the emulator in (28) has the vector (q ,x I as its input 
and n outputs, and a collection of simple computing elements ® m (q,x) are used to 
approximate the function f m (q,x) . 

3.3 Fundamental matrices. 

For the sake of simplicity in the following, the matrix derivative of W m (q) with q e M n will 
be denoted as 



eAW R ) ( 29 ) 



WPm(q)A 




9q 


'W m (q) 

V 9q« J 



3.3.1 Inertia Derivative Matrix M D (q,x) 

3.3.1.1 Real Case 

It is possible to express the inertia derivative matrix as consisting of two terms, the 

approximation component M D (q,x) and the reconstruction error E D (q,x) . 

UoM-t^ + t?^ PC 

j=l u 4j j=i a 4j 

Following the discussion given in the ideal case, the approximation term M D (q,x)y can be 
expressed in the regression form: 

M D (q,x)y = <D D (q,x,y)§ m (31) 

3.3.1.2 Ideal Case 

The following lemmas are helpful for characterizing the inertia derivative matrix in LIP 
form. 

Lemma 1 (LIP form of M D ). For arbitrary vectors x,yeK n , qefl Cl°, the inertia 

derivative matrix can be written in the LIP form as follows 

M D (q,x)y = <D D (q,x,y)§ m (32) 

where § m eR is the parameter vector of the generalized inertia matrix and 
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O d G -Mnxi (R) a regression matrix: 

d> D (q,x,y) = (y T ®I n ) M ;(q,x) 
i|/(q,x) = 



xV^ 



dq 



(33) 



Proof: This fact can be straightforward proved by resorting to the definition of M D given 
above: 



m d q,x y = 2]— ^^x yi 



'A dO m {q,x) ) 



S„ 



x^E^y 1 



fci %i 



§ m = <D D (q,x,y)§ m (34) 



By closely examining the structure of O d and using the property 



fcl ^i V ' o»q 



(35) 



it is easy to conclude that 



<D D (q,x,y) = x T ®((y T ®I n )^)' 



x® 



^m(q) 



9q 



>»!»; 



x® 



^m(q) 



9q 



>»!»: 



y T ®I n W(q,x) 



(36) 



3.3.2 Fundamental Matrix M v (q,x) 

3.3.2.1 Ideal Case 

Lemma 2. For arbitrary vectors x,y£l n , qe£2 CM n , the inertia velocity matrix can be 

written as 



M v (q,x)y = <D v (q,x,y)j; n 



(37) 



where <D v (q,x,y) = Er= 1 i5 # Zl x i . 

Proof: Owing to the commutation property of fundamental matrices the result is direct. 
Remark: It is easy to show that the Jacobian matrix <t> v (q,x,y) also satisfies the 

commutation property, <t> v (q,x,y) = D (q,y,x) .This is an immediate consequence of the 

commutation property of M v (q,y) and M D (q,x) as given in the identity 3. 
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3.3.2.2 Real Case 

The inertia velocity matrix M v (q,x) can be seen as consisting of an approximation term 

M v (q,x) and a reconstruction error E v (q,x) . The approximation term M v (q,x) can be 
written in terms of the Jacobian of <D V (q,x,y) as 

M v (q,x)y = <D v (q / x,y)§ m (38) 

and the error E v (q,x) is derived from the gradient of the inertia error £ m (q) . 



Ev(q ' xH S^- x ^S^T Xl 



(39) 



3.3.3 Transpose of the Inertia Derivative Matrix 

Now that it is known how M D can be written in LIP form, the main challenge, now, is to 
extend this result to its transpose. This is more difficult since involves a permutation matrix 
as stated in the following lemma. 

Lemma 3: Let x,yeR n , q&Q CI" arbitrary vectors with appropriate units and P a 
permutation matrixl defined as follows 



P(n,m) = ££Ei|® E ji 

i=i j=i 



(40) 



where E* = ejej is a unit matrix having 1 in position (i,j) and all other entries are zero. 
Under these conditions, the term M D (q,x) can be written in LIP form as follows 



M£(q,x)y = (x T ®I n )p(n,n) ¥ (q,y)S r 



(41) 



Proof: By virtue of definition 1 and lemma 1, the generalized force M D (q,x)y can be 
directly derived in the LIP form which can be detailed into 



M£(q,x)y = $je, 

i=l 

= (l n ®x T ) 



x x9M(q) y 



3q ; 
dq 



, T N<9M(q) 

-( I n® xT )^y= 



?m= In®x' 



8q 



(42) 



P(m,n) is a matrix of zeroes and ones for which P(m,n) =P(m,n) =P(m,n) 
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With reference to the property B®A = P(m,p) (A®B)P(n,q) for all AeX mxn (M) and 
B e _A/L, xq (R) , (see Corollary 4.3.10., p. 260, in (Horn & Johnson,1999)), the term (x T ®I n ) 
can be commutated as 

(x T ®I n ) = P(n,l) T (l n ®x T )P(n,n) (43) 

The last identity can be simplified further by exploiting the structure of the permutation 
matrices. In particular, it is easy to show that P(n,l) = P(l,n) = I n (see problem 18, section 
4.3, p. 265, in (Horn & Johnson, 1999)), which leads to 

(l n ®x T ) = (x T ®I n )P(n,n) (44) 



Therefore, 

M£(q,x)y = (x T ®I n )P(n,n 



y^ 9 ^) 



dq 

®I n )p(n / n) ¥ (q,y)? m 



(45) 



Remark: The permutation matrix in the last lemma can be also written as 

P(n,n) = fj i ®l n ®eJ=fy?®I n ®e i (46) 



For more details the reader is referred to the problem 21, section 4.3. p.286,in (Horn & 
Johnson, 1999). 

3.3.4 Coriolis/Centripetal Matrix 

On the basis of the description of M D and Mj in LIP form, the skew-symmetric matrix J can 
be also represented as LIP. 

Lemma 4: Let x,y G K n , q£Sl„C R n arbitrary vectors with appropriate units. The skew- 
symmetric matrix J(q,x) can be expressed is linear in the parameters, 
J(q,x)y = %(q,x,y)U 

where $j(q,x,y) = (y T <g> I n )(I n 2 - P(n,n))a|;(q,x) . 
Proof: It is straightforward that 
J(q,x)y = M D (q,x)y~MS(q,x)y = (y T ®I n )(I n2 -P(n,n))^(q,x)U 

Lemma 5: For arbitrary vectors x,y e R n , q e Q„ C M n , the inertia velocity matrix can be 
written as 

M v (q,x)y = $ v (q,x,y)£, m 
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where <& v (q,x,y) = ElU^^Xj . 

Proof: This is direct. 

Remark: It is observed the following identity: $ D (q,x,y ) = c& v (q,y,x) , which is consistent 
with the commutation property. 
Remark: An alternative way for J is obtained by resorting to the commutation property: 

M v (q,x)y-M£(q,x)y = M D (q,y)x-MS(q,y)x = 

= (x T ®I n )(I n2 -P(n,n))i(q /y )U=J(q,y)x 

Remarkably, the above lemmas can be conveniently used to write the Coriolis/ centripetal 
matrix in LIP form. 

Proposition 1 (Coriolis/Centripetal matrix in LIP form): Let x,yeM n , q £ Q,„ C R n 

arbitrary vectors, the Coriolis/ centripetal effect C(q,x)ycan be linearly factorized as a 
regression matrix ( l? c (q,x,y) and a parameter vector £, m , i.e.C(q,x)y = < t > c(q/ x /y)£.m/ 
where $ c is given by 

*c(q,x,y) = i[(x T ®I n )^(q,y) + (y T ®I n )(I n2 -P(n,n))4.(q,x)] 
Proof: By restoring to the LIP form of M v and J , matrix C can be written as 

C(q,x)y = -(M v (q,x)y + J(q,x)y) = 



i[(x T ®I n )^(q,y) + (y T ®I n )(I n2 -P(n,n)) 1 l;(q,x)]^ 



3.4 Model Errors 

The dynamic model of the robot manipulator is allowed to be imprecise since the nonlinear 

function f(q,q,x,y) = M(q)x + C(q,q)y + G(q) , (where x,y£l" are arbitrary vectors 
usually with units of acceleration and velocity respectively),is not exactly known. The 
imprecision comes from unstructured uncertainties, namely modeling errors caused by the 
truncation of the Gaussian expansion series. A detailed description of the approximation 
errors is demanding from a modeling viewpoint. To point out the fundamental aspects of 
error modeling, it is convenient to express the total error as composed by three terms, 
(Mulero-Martinez, 2007a), 

E = E m (q,x) + E c (q,q,y) + E G (q) 

By referring to the expression of the Coriolis/centripetal matrix in (Mulero-Martinez,2007a) 
from the fundamental matrices M D and M v , the Coriolis/centripetal errors E c (q,q,y ) can 
be written as 

E c(q,q,y) = 2( Eo ( q ' c i)~ E D( q ' c i) + Ev ( q ' ( i))y 
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The error term E D (q,q) is the approximation error associated with the fundamental matrix 
M D (q,q) and E v (q,q) regards with the velocity matrix M v (q,q) . For the sake of 
simplicity the gradient of the inertia error £ m ( q ) will be denoted as 



<3q ldqi'"''dq n . 



m ^ ^<-m wo m ,- mnxn 



G 



These errors can be expressed in terms of the gradient of the inertia error e m ( q ) as follows 

Bn(q4) = E^)qe? 

't (47) 

For the following derivation, it is worth rewriting the mathematical errors in a more suitable 
form using the Kronecker product. This is written down in the following properties. 
Claim 3: The linear transformation E D (q,q)q r can be formulated in terms of the 
Kronecker product as 

E D (q,q)q r = *|ia)(q r ®q) (48) 

Proof: The proof is derived directly from the definition of E D ( q, q ) 

Claim 4: The linear transformation E v (q,q)q r is expressed in terms of the Kronecker 
product as 

Ev(q,q)q r = ^f^(q®q r ) (49) 

Proof: This fact can be trivially proved from the definition in equation (47) 

17 / • x- V^^ £ m(q)- ■ 9 e m (q). . , 

E v (q,q)q r = E g q ^^ = da ^ ^ ^ 

Remark: Equations (48) and (49) are not the same since the Kronecker product is not 
commutative, i.e. ( q r ® q ) ^ ( q ® q r ) . 



4. Design of the Adaptive Controller. 

4.1 Error Dynamic Equation 

In order to manage equilibrium points at the origin, it is necessary to make a coordinate 
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transformation so that a position error variable is considered, e(t) = q,j(t) — q(t) . Thus, 
convergene of trajectory q(t) to the desired trajectory qa(t) can be analysed observing 
position error trajectories e(t) close to the origin in the phase space. The objective of the 
controller is both the stable tracking of trajectories and the rejection of disturbances. A good 
tracking performance means that the error converge to zero (asymptotic stability) or to a 
finite value, lim t _, 00 e(t) = E < oo . This idea is also applied toq(t) and q(t) because a 
position trajectory is given by three quantities (q(t),q(t),q(t)) . Measurements of 
velocities are easy to get by tachometers, but sensors of acceleration are noisy and are not 
used for implementation in robotics field. This fact conjures up to use a filtered error signal, 
r(t) , that is a derivative filter or PD (Slotine & Li, 1991): 

r(t) = q r (t)-q(t) (50) 

From (50) it is shown that r(t) is a measurement of error between real velocity q(t) and 
reference velocity q r ( t ) . This reference velocity must not be confused with the desired 
velocity q^ ( t ) . In fact, reference velocity is defined as follows 

q,(t) = q d (t) + Ae(t) (51) 

where A is a diagonal matrix of design parameters with big positive elements so that the 
system is BIBO stable. This matrix allows to filter errors so that no acceleration of errors 
e(t) will appear in the error dynamic equation. The definition of filtered error r(t) in 
terms of position and velocity errors can be obtained from (50) and (51) 

r(t) = e(t) + Ae(t) 
Substituting q(t) from (50) into the plant, the error dynamic equation is derived. 

M(q)r = -C(q,q)r + f(x)-T + T d 

where f (x) = M(q)q r + C(q,q)q r +G(q) + F(q,q) stands for non-linear terms to be 
compensated. This non-linear function could be parametrised by x =(q ,q ,q r ,q r ) or 

using the definition of q r (t) in (51) by x T = I e T ,e T ,qJ,qJ,qJ J . 

A structural property of robot manipulators is the linearity of parameters (LIP) (Craig, 1989), 
(Sciavicco, 2002). This means that non-linearities can be split up into a parameter vector P 
and a vector of basis functions "5 ( x ) . Therefore the non-linearity function f ( x ) can be 
expressed in this sense adding a term of error e . 

f(x) = *(q,q,q r ,q r )P + e 
Linearity of parameters (LIP) is a first assumption in most of the adaptive controllers. 
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Proceeding in a similar way, the approximation of the non-linear function using estimation 
of the parameters P can be represented by means of linearity of parameters. 
f(x) = *(q,q,q r ,q r )P 



4.2 Controller Structure, Control Law and Updtating Law. 

Up unto this point, LIP property has been analyzed via fundamental matrices. The control 
approach employs an inertia-related linearization approach, i.e. a conservation of energy 
formulation, as an attempt to derive update laws and control laws. To be specific, it is 
required to define an inertia-related Lyapunov function in the stability analysis which 
utilizes physical properties inherent to a mechanical manipulator (such as those presented 
above). Thus, the stability of the tracking error system is ensured by formulating the 
adaptive update rule and by analyzing the stability of the tracking error system at the same 
time. It is well known that dynamic models even though quite complex are anyhow an 
idealization of reality. Specifically, robots show uncertainties that are mainly found from 
two sources: variability of parameters and nonlinearity terms in the system. One way of 
dealing with parametric uncertainties would be to use the inertia-related approach, see 
(Lewis et al., 2003). The benefits of this approach as compared to others is that avoids a 
direct measurement of acceleration and the invertibility of the generalized inertia matrix, 
which are restrictions of some controllers such as those inspired in the adaptive computed- 
torque approaches. 



NON-LINEAR INNER LOOP 



ir ir ir 
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CONTROL 
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Fig. 1. Adaptive Control Structure 
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The controller consists of three terms: a PD-controller to guarantee a good tracking 
performance, t„,j = K v r = K v (e + Ae) , a compensator of non-linearities, T n ] =f and a 
robust controller to absorb unmodelled dynamics T r . 

x = K v r + f + T r (52) 

where K v = K^ > is the gain matrix. 

The control structure appears in figure 1. In this scheme, two loops can be spoted: an outer 

loop to track signals and an inner loop to compensate non-linearities. The inner loop is 

driven by an adaptive control and the outer loop is driven by a robust and PD terms. 

An important feature of this class of controllers is that of being based on the all-important 

closed-loop error dynamics, which results from the substitution of the filtered tracking error 

into the robot dynamics 

M(q)r = -C(q,q)r + f(x)-T = -C(q,q)r-K v r + f(x)-T r (53) 

where f(x) = f(x) — f ( x ) stands for the functional estimation error with x being a vector 
of appropriate variables as shown below. The nominal nonlinearity f can be computed as 

f(q,q,q r ,q r ) = M(q)q r + C(q,q)q r + G(q) 

where q r is the reference acceleration, q r (t) = qa(t) + Ae(t), and q r the reference 
velocity, q r (t) = qd(t) + Ae(t) . The nonlinearity fcan be conveniently partitioned into 
several smaller terms, resulting into an added controller structure 

f(x) = f m (x) + f c (x) + f g (x) 

where L(x) = G(q) . The terms f m and f c can be defined in terms of q r and q r as 
follows 

f m(q,q r ) = M(q)q r =$ m (q,q r )t, m 
fc(q,q,q r ) = C(q,q)q r =* c (q,q,q r )£ >m 

f g (q) = G (q) = *g(qKg 

where ^ m 6 R " , Let 1 are parameter vectors and $ m , < & (■,<&„ regression matrices. 
Theorem 1: Let the desired trajectory qa(t) bounded by q B , i.e. ||qd(t)| < qs .Suppose 
that the approximation error e and unmodeled disturbances tj ( t ) are upper bounded 
by e N and d B respectively. Let the control law given by (52) with a robust term, 
T r (t) = K r sgn(r) where K r — diag(k r . ) > with k r . > e n + d B . The next parameter 
updating laws are considered 
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L = r m (*m (q,q r ) + *c (q,q,q, ))r = 

= r m f(q r ®*J l (q))-[i|) T (q / q r )(q®I n ) + i)j T (q / q)(l n 2 - P(n,n))(q r ® I n )]jr 

with T m = rj, > , r„ = rj > and rf = V] > symmetric positive-definite constant 
matrices. Then tracking error r(t)->0 as t — ► oo and parameters £, m and £,„ are 

bounded I M\ < £, B J . It can be concluded that e G £§ n Z^> / is continuous, e(t)— >0 

and e(t) — > as t^oo; and t is bounded. 
Proof: Select the following inertia-related Lyapunov-like function: 

V =\r T M{q)r+^ &r£U + \ 1^% (54) 



where | m e R m , Lei" are the parameter error vectors, r m 6Mi mX i m (R) , 
T g e M] x i (R) diagonal, positive-definite matrices. Note that the term ^-r T M(q)r 

stands for the kinetic energy with filtered error instead of joint velocities, and 4^ T r -1 ^ is 

the energy attached to the non-linear terms to be compensated. It is noted that in (54) , an 
energy for Coriolis/centripetal terms does not appear because these terms are obtained from 
inertia matrix approximation using Christoffel symbols of first kind. Matrices T m , r„ 

and T f are diagonal and their non-null terms represent the learning rate of parameters to 
nominal values, for inertia and gravitational respectively. 
Differentiating (54) with respect to time gives 

V = r T M(q)r + ir T M(q)r + ^r m ^ m+ ^ rg l^ (55) 

From (55) it is clear that we must substitute for the variable r ; therefore we must write the 
robot equation in terms of the variable r . Using (53) the robot dynamics can be rewritten 
as 

M(q)r = -C(q,q)r-K v r + f m (q,q r ) + f c (q,q,q r ) + f g (q)-T r 

The functional estimation error f ( x ) can be expressed in terms of regression emulators as 
indicated below 

L (q4r ) = * m ( q>q r )i m + E m ( q,q r ) 

f c (q,q,q r ) = $ c (q,q,q r )£, m + E c (q,q,q r ) (56) 

f s (q) = < MqKg + £ g (q) 
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Owing to the passivity property of robots M(q) — 2C(q,q) is a skew symmetric matrix 
and its quadratic form is zero: 

V(r) = -rXr+r T (f m (q,q r ) + f c (q,q,q r ) + f g (q)) + r T T r + |Tr m i| m + qr g ^ g (57) 

This is the same type of parametric separation that appears in the formulation of the 
adaptive computed-torque controller; however, note that regression matrices are not a 
function of joint acceleration q . Now, substituting (56) into (57) gives 



V = -r T K v r + r T ($ m (q,q r ) + $ c (q,q,q r )) + ^ m T n 



£m + 



r T <Mqm g r g 



+ r (e(q,q,q r ,q r ) + T d ) 



where 



e(q,q,q r ,q r ) = E m (q,q r ) + -(E D (q,q r )q r -ES(q,q r )q r + E v (q,q)q r ) + E g +e f (58) 

Now, the strategy consists of making zero the terms in brackets, which results in the 
following adaptive update rules: 



L = r m (*T (q,q r ) + $J (q,q,q r ))r = 

= r m ((q r ®v[/T (q) ) 1 .[ t T (q ^ r)( q 8ln) + 4 ,T (q ^ )(In2 _p (n(n) ) ( q r8ln) ]| 



^ = r g*g(q) r 



As long as e(q,q,q r ,q r ) and T d (t) are bounded, V(r) is also upper bounded. 



V(r)<-K Vm J|r|| 2 +r 1 (eN+d B )-r 1 K r sgn(r) 



(59) 



If K r . > e n + d B , and using lemma 6 in the appendix, r T (— K r sgn(r) + e N + d B ) < , so 
that, V(r) is negative defined 

V(r)<-K„ J|r|| 2 



Hence, V ( r ) is a Lyapunov function, V ( r ) > and V ( r ) < so that stability in the sense 
of Lyapunov is guaranteed. 



a min (K v ) jVrdx < J*r T K v rdT < V(0) 
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where a min (K v ) is the minimum eigenvalue of K v .Since V(0) and a min (K v ) are 
positive constants, r(t) is a signal of finite energy (r(t) 6 C{ ) and this implies that is 
bounded. From lemma 7 in the appendix, ee/Jfl C^, , e is continuous and e — » as 
t — > oo , and ee/J . On the other hand, V(r) is semidefinite negative, so that 
< V(r(t))< V(r(0)) Vt > .Since V is lower bounded by zero and V is 
nonpositive, it follows that V approaches a finite limit, which can be written as 



/oo . 
V(r) 



dt < 



and this implies that V(t)e/J 30 and that £ m and iL are bounded. Observing that 
M (q) , parameters t^ m and £,„ , and regression functions $ are bounded, and 
r(t) £ ££ / qd'<id'qd' T r G LSo , then from error filtered dynamics as given by (53) the 
boundedness of r and -r(t) is verified ( r, t e L^, ). From the theorem 2 in the appendix 
and given that e £ C{ , it follows that r G E{ n C^ , r G C{ , r is continuous and 
r ( t ) — ► as t — > 00 . Hence, e — > as t — > 00 . 

4.2 Semi-global Stability. Initial Conditions Dependence. 

At this point a number of comments are in order. First of all it is interesting to explore in- 
depth the initial conditions that must be satisfied by both the position errors and the 
velocity errors so as to guarantee the approximations to be valid. Thus, in the following 
remark these conditions are discussed from the Stone-Weierstrass theorem and from some 
results concerning with the stability of non-linear systems. The importance of developing 
this analysis stems from the fact that the approximations are only valid over a compact set 
whose size depends on a given desired accuracy. 
It is assumed that the joint i has physical limits q m ; ni and q maXi so that the workspace 

fi q can be described as [qmin^qmaxj ] x --- x [qmin n /qmax n ] . For the subsequent discussion, 
it is worth considering the following weighting norm 

where p e R n is a given vector. As a result, the compact set Q„ can be represented in the 
form of an n — ball, i.e. 

B(q ,p) = {qeir|||q-qo|| (VDO <l} 

where 
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qo, - - 



2 
Pi = 2 

Remark As it was claimed above, the desired trajectories remain bounded for all time. This 
means that the vector qa(t) is in the compact set Q^ = {q,j e M n ||q(j || < q B } for an 
appropriate upper bound q B . From the Stone-Weierstrass theorem, it is possible to find an 
emulator with a sufficient number of nodes and, with suitable parameters, satisfying the 
following condition 

l f m (q.q r ) - * m (q,q r )i m II < l E m (q,q r )|| 

for a given vector q r , and over a compact set Q m including the workspace of the robot 

manipulator. In this, it is convenient to recall that E m (q,q r ) = e m (q)q r where 

e m ( q ) G M nxn denotes the estimation error for the inertia generalized matrix. Note that the 

compact set fi q denotes an operation region of interest in which the system works on. This 

implies that the obtained result is regionally stable in the sense that all the states q(t) 

must be guaranteed within the compact Q„ . Furthermore, this compact set should include 

the space of desired trayectories, i.e. fl d C Q 

In a similar way, it can be stated the following validity regions for the approximation of 
fundamental matrices M D and M v 

ll M D(q,q)q r - * D (q/q/<ir )im II < II e d (q,q)q r I 
l M v(q,q)qr-*v(q,q,qr)U||<||E v (q,q)qr|| 

| m d (q,q)q r - (q ® I„ )P(n,n)4»(q,q r )i m I < |E D (q,q)q r | 

for a given vectors q, q r e K n and for all q in Q„ describing the validity region. It is 

clear from this discussion that the ideal parameters are derived as the following 
optimization problem 

£,m- min {||f m (q / q r ) + f c (q,q,q r )-$ m (q,q r )^ m -$ c (q,q,q r )^ m ||} 

qen q 

In the last expression L m denotes the number of nodes required for the inertia 

approximation. 

The same observations can be done for the gravitational vector G ( q ) . Thus, the validity 

region 0„ for the approximation of this term is determined by the condition 

||G(q)-4g(qHgI<||e g (q)| 

for all q £ Q„ . Evidently, for this region to be useful, it should be included the workspace 
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fi q . Just like for the inertia terms, the ideal parameters are derived as 

^= W ^»{1 G (1)-*«(«1)^|} 

qe!l q 

It is known that the error e(t) is connected to the filtered tracking error r(t) via a 
transfer function G r (s) = sl + A with A e M nxn a Hurwitz matrix. 

From lemma 8 in the appendix, it should be clear that being G r ( s ) a strictly proper, 
exponentially stable transfer function, then r e £% =>• e e £% n £» / e e ££ , e is 
continuous and e(t) — > as t — ► 00 . If in addition r — > as t — > oo , then e — » . 
Furthermore, it is assumed that the filtered tracking error r G C^, , i.e. there exists a 
constant b, el such that ||r||<b r . Actually, this condition should be guaranteed by the 
controller. If the differential equation describing the closed-loop filtered tracking error 
dynamics is solved, it is easy to see the following relationship 



lit) = e^ A(Mo) e(t ) + r e- A(t - T) r(T)dT 

J t„ 



Due to ||r || ^ < b I/OC , being b roo 6l a constant, it is straightforward to derive an upper 
bound for the error norm 

l^t)U<|e(to)IU + ^^|e- A ^)-l|U<|e(t )| U+ ^^ (60) 

The universal property of approximation holds so long as q(t)efL or equivalently 
whenever |q(t) — q | <1 ■ Applying the triangle inequality to (60) and from the 
boundedness of the desired trajectory (i.e. |q d (t) — q || < q d/3C )/ it is not difficult to 



write the following ||q(t) — q | p/00 < qa.^ + ||e(t )|| P;OC + ^ — r -^j < 1 ■ 



X min {A} 
Note that owing to f! d C Q , the bound q d/00 satisfies the condition q doo < 1 . Now the 

radius b r;00 is determined, 



br.oc, < X min { A } ( 1 - q d/00 - 1| e ( t ) || P/00 ) (61) 

As a consequence, the initial filterd tracking error must satisfy the condition |r(to )|| < b roo 
for the constant b r >00 given in (61) . 

It is known that |r(t )| P/OO < ||e(t )|| P/OO +X max { A }||e(t ) || P/00 and then, the condition 
||r(to)|| < b r /00 is held whenever 
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l|e(to)|| p , 00 +X max {A}|e(t )|| P/00 <X mta {A}(l-q d , 00 -||e(t )|| p , 00 ) (62) 

It is concluded that the initial errors e ( t ) and e ( t ) must verify the inequality (62) , 
where the constants A and qa.^c are known a priori. 

Remark If the robust term is removed from the control law, it is necessary to account for an 
additional assumption of persistence of excitation (PE) on the signals $ g and $ m . In this 

case, the variation of the energy is upper bounded in the following way 

V<-X min {K v }||r|| 2 +||r||( EN +d B ) 

Obviously the energy is decreasing as long as the size of the filtered tracking error remains 
greater than a constant, i.e. 

e N +d B 



X m in { K v } 



As it was pointed out in the previous discussion, the validity of the approximation holds 
throughout whenever r 6 fi r = jr 6 K n ||r|| x < b I/00 } , being b r <oc given in (61) . Under 
these considerations, it is easy to derive a suitable selection for the derivative gains as 

X m in{K v }>: 



X min {A}(l-q d , oc -||e(t )|| piDci ) 

Unfortunately, the tracking errors do not vanish, but are bounded by small enough values. 
Since ||r|| <b roo , the errors e and e are also guaranteed to be bounded. Additionally 

to this result, the boundedness of the desired trajectory also implies that the states q and 

q are bounded. To complete the analysis the boundedness of the parameters is now 

explored. To this purpose, it is worth considering the dynamics relative to £, m and £, g 



n = -r m ((q r ®*m(q))i[^ T (q,qr)(q®In) + ^ T (q,q)(I n 2 - p ( n ' n ))(qr®In)])r 

y m = *m(q,q r )£m 
y s = *g(q)£ g 

It can be shown that the boundedness can be obtained by reformulating the problem in 
terms of the Kronecker product. This consideration leads to the following vector dynamics 
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'r m ((q r ®*T (q) )I^T (q ^ r)( q 8ln) + ^T (q ^ )(In2 _ p(n ^ n) ) ( q r8ln) ]) r - 

r s*s(q) r 

'i 



y 
\y s ) 



Q(q,q,q r ,q r 



*U 



where the matrix Q is defined as 



Q4 



*m(q,q r ) o 
*g(q). 



Now, the PE condition is equivalent to the persistence of excitation of $g(q) and of 
$ m (q,q r ) , and consequently to the uniform complete observability of this system. From 
the lemma 9, the boundedness of y m (t) , y g (t) , and r(t) insures the boundedness of 

£, m and of (L , and then the boundedness of £, m and of (L . With reference to the 
theorem 3, due to ||r|| is greater than a constant, it can be asserted that the system is 
ultimately uniformly bounded (UUB). 

Remark: Defining the state x = (r 1 ,^,!! J the energy can be written as 



where 



V(x) = -x T Px 

v 2 



M(q) nxnLm nxnLg 

OnL m xn *m(q>q r ) nLmXnLg 
OnUxn nL xnLm *g(q) 



In this case the energy is lower and upper bounded, 



^l|x|| 2 <V(x)<i||x|| 2 



where the constants \ 1 and X 2 are defined as follows 



\i=min \ min {M(q)}, 
X 2 =max|\ max {M(q)} 



^maxi^m/ X max |lg| 

1 1 

^min Umi ^min \ *■ g J J 

Applying the passivity property of robot manipulators on the variation of the energy with 
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respect to time leads to the following expression 



V = -r T K v r + r T (e(q,q,q r ,q r ) + T d ) + 



.T 

r T * m (q,q T ) + | m ri 1 



,T 

r T $ g (q) + Lr g 1 



Folding the parameter updating law (62) into the last expression, the variation of the 
energy becomes 

V = -r T K v r + r T (e(q,q,q r ,q I ) + T d ) 

Assuming that the external disturbance torque and the functional reconstruction error are 
uniformly bounded, i.e. ||T ci ||<d B and |e(q,q,q r ,q r )|| < e B , the power will be bounded 
by 

V<-X min {K v }||r|| 2 +(d B+eB )||r|| 



with X m i n {K v } representing the minimum eigenvalue of the gain matrix K v . Owing to 

(d B +e B ) 

x ml „{K v ; 



e B and d B are constants, V is semidefinite negative as long as |r||> } ',?. .Hence, 



(dB+EB) -<||x(t)||<oc 



\nin { K v } 

Invoking the theorem 4 in the appendix, it is concluded that 

||x(t)|<|^4./Svt e [to+T,oo) 

where < T < oo .In this way, it is stated the ultimately uniformly boundedness of x . 
Remark: In the ideal case, when e N = d B = , it is easy to show the asymptotic 
convergence. In this case the variation of the Lyapunov energy in (59) turns to be 

V(r) = -r T K v r 

Note that in this case no robust term is necessary to be introduced into the control law since 
there is no functional reconstruction errors nor external disturbances. It can be shown that 
the second differentiation V(r) = — 2r T K v r is also bounded and uniform continuity of 
V(r) is guaranteed. Since V(r)>0 , V(x, t) uniformly continuous and non-positive, 
involking lemma 7 in the appendix, it is easy to show that V vanishes as t goes to 
infinity. Indeed, by using Barbalat's lemma it can be concluded that V(r) — » as t — > oo 
and that r ( t ) — ► . Hence, e ( t ) — » as t — > oo . 

The parameter tuning algorithm is hardly a continuous-time backpropagation algorithm. 
These parameters are initialized to zero so that there is no preliminary off-line phase and in 
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the first steps of working the controller behaves just as a PD controller. If the gain K v is 
considered to be large, the closed loop error remains bounded and this implies the 
stabilization of the whole system. This initialization is very important since it is not 
necessary to choose initial parameters that make stable the system. It is well known that this 
task is very complex to do. 




Fig. 2. Two-Link Planar Elbow Arm 



5. Results 

A planar two-link arm robot is used here as a platform to demonstrate the effectiveness of 
the adaptive controller. These kind of robots are used widely in the literature to get proof of 
the effectiveness of the controllers and appears in figure 2. Dynamics can be found in the 
literature (Spong & Vidyasagar, 1989), (Lewis et al. ,2003) and no friction is taken into 
account in the model. The generalized inertia matrix is given by 



M(q) = 



m i 1 c 1 + m 2 1 ?+ I i m 2 1 i 1 c, cos (q2-qi! 
m 2 1 i 1 c, c o s (q 2 - qi ) m i l l, + h 



(63) 



where q = (qi,q2) is the joint variable, rrij , m 2 are the masses of the links, lj , 1 2 the 
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length of the links, l c , l c the position of the center of mass respect to the frame attached to 
the link and l 1 , I 2 are the inertia about the Z-axis for each link. 

The construction of the approximating function in the controller is illustrated here. From (Ge 
et al., 1998) a superset of basis functions cp m (q) for M (q) is found and this set described 
completely the robotic system. 

c Pm(q) = ( 1 ' s i n (qi)' sin (q2)' c o s (qi)' c o s (q2)) e» 5 (64) 

In practise, only a subset of elements is necessary to describe the whole system and this 
means that M(q) is over parametrised. From (64) the activation function for inertia 

matrix, Ym(q'qr) is built as Ym(q'q r ) = q r ® c Pm(q) ■ The dimension of Ym(q'qr) means 
that the number of nodes required to approximate inertia matrix is 10 . 
Coriolis-centripetal matrix is carried out from (58) and this implies to calculate the Jacobian 

matrix i|r(q,x) = Ya=i x® g e ; . This Jacobian is applied to vectors of dimension 2 in 



3q, 

order to work out the Coriolis/ centripetal matrix so that the number of nodes is also 10 . 
The desired trajectory is chosen as a periodic sinusoid with amplitud 1 and frequency 1 
rad/sec. 

q dl (t) = sm(t) 

q da (t) = co8(t) [ ' 

This provides bounded signals for position, velocity and acceleration. It is assumed that 

initially there is no knowledge about the system so that all the weights are initialized to 

zero. 

The robot parameters are l 1 = 1 2 = lm and the mases are m 1 = 0.8kg , m 2 = 2.3kg . The 

gains of the controller were chosen as K v =diag(20,20) , T m =T =T £ =diag(50,50) , 
A = diag(5,5) . It is assumed the next initial conditions for signals and estimations, 
q(0) = , q(0) = , m 1 (0) = , m 2 (0) = . In this scheme all the parameters are 
initialized at zero so that there is no preliminary off-line learning phase. The signal tracking 
and weight tuning works together on-line. The main benefit of this initialization is the 
independence of the controller as regards the initial parameters skipping the task of finding 
initial stabilizing parameters as necessary in other control techniques. 

The whole system was simulated in SIMULINK using Runge-Kutta method with an 
integration fixed step of At = 10~ sec and a simulation range from to 20 sec. The response 
of the controller with parameter tuning (e.g. Theorem 1) appears in figure 3. 
It is noted that no knowledge of the dynamics is needed for adaptive control and a good 
tracking performance is obtained as observed in figure 4. Position error signals remains in a 
bound band of ±5% respect to the equilibrium point. In this figure an exponential 
convergence response of the system is showed with a setting time about 5 seconds. It is 
possible to improve the tracking performance by increasing the gains of the controller. 
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Therefore, it is concluded that the designed neurocontroller provides a good tracking of 
desired trajectories. 
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Fig. 3. Response of the Adaptive Controller with Gradient- Type tuning. Actual and desired 
joint angles. 




Fig. 4. Response of the controller with gradient-type parameter tuning. Representation of 
tracking errors 
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6. Appendix 

Lemma6:For K = diag(K u )e R nxn and d = (d 1 ,d 2 ,...,d n ) T e R n , if u = ~Ksgn(x) and 
kii>|di| then x T M(u-d)<0 (Ge et al., 1998). 

Lemma 7: Let V(x,t) be a Lyapunov function so that V(x,t)>0, V(x,t)<0.If V(x,t) 
is uniformly continuous (Lewis et al., 2003), then 

V(x,t)^0ast^oo (66) 

The following theorem is very important in control of non-linear systems, and is due to 
Desoer and Vidyasagar, cf . (Desoer & Vidyasagar, 2008) 

Theorem 2: Let the closed-loop transfer function H(s)e M nxn (s) be exponentially stable 

and strictly proper, and h (t) the corresponding impulse response (obtained by evaluating 

the inverse Laplace transform of H(s)). If u e £% , then y = h*u e £% n£^, , ye £2 > Y 

is continuous and y(t)— »0 as t^oo , where h*u denotes the convolution product of 

h and u . 

On the basis of this theorem, it is possible to state the following lemma, (Ge et al., 1998). 
Lemma 8: Let e(t) = h(t)*r(t) , where h = CT |H(s)l and H(s) is an nxn strictly 

proper, exponentially stable transfer function. Then re ZJJ =>> e G ££n £^ , e e ££ / e i s 

continuous and e(t)— >0 as t— >oo . If in addition r^O as t — > 00 , then e^O . (Ge et 

al., 1998). 
Theorem 3 (UUB by Lyapunov Analysis): If for system 

x = f(x,t) + g(t) (67) 

there exists a function V(x, t) with continuous partial derivatives such that for x in a 
compact set SCI" 

V(x.t) is positive definite, V(x.t) > 

. (68) 

V(x,t)<0for ||x|>R 

for some R > , such that the ball of radius R is contained in S , then the system is UUB 
and the norm of the state is bounded to within a neighborhood of R . 

The following theorem is a modified version of the uniformly ultimately boundedness 
theorem of Corless and Leitmann, cf. (Corless & Leitmann, 1981). For more insights the 
reader may refer to theorems 1 and 2 in (Dawson et al., 1990) or the theorem 2.15. p. 65 in 
(Qu, 1998). 

Theorem 4: If V is a Lyapunov candidate function for any given continuous-time system 
with the properties 

X 1 |x(t)| 2 <V(x(t))<X 2 |x(t)| 2 (69) 
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V(x(t))<0,ifn 2 >|x(t)||>ih (70) 

where 

lx 2 



then 

llxfttlkn-, , 




(71) 



x(t)|<HiJ^-Vt6[to+T,oo) (72) 



where T is a finite positive constant. 

The following lemma allows to connect the uniform complete observability (UCO) to the 
boundedness of the states, (Lewis et al., 1999). 

Lemma 9 (Technical Lemma): Consider the linear time-varying system (0,B(t),C(t)) 
defined by 

x = B(t)u 

r(!\ (73) 

y = C(t)x 

with xel" , ueK m , yeK p and the elements of B(t) and C(t) piecewise 
continuous functions of time. Since the state transition matrix is the identity matrix, the 
observability grammian is 

N(t,t )=rC T (T)C(x)dT (74) 

u to 

Let the system be uniformly completely observable with B(t) bounded. Then if u(t) and 
y(t) are bounded, the state x(t) is bounded. 
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1. Introduction 

The mechatronic design of a humanoid robot is fundamentally different from that of 
industrial robots. Industrial robots generally have to meet requirements such as mechanical 
stiffness, accuracy and high velocities. The key goal for this humanoid robot is not accuracy, 
but the ability to cooperate with humans. In order to enable a robot to interact with humans, 
high standards are set for sensors and control of its movements. The robot's kinematic 
properties and range of movements must be adjusted to humans and their environment 
(Schafer, 2000). 

1.1 The Humanoid Robot ARMAR 

The collaborative research centre 588 "Humanoid Robots - learning and cooperating multi- 
modal robots" was established by the "Deutsche Forschungsgemeinschaft" (DFG) in 
Karlsruhe in May 2001. In this project, scientists from different academic fields develop 
concepts, methods, and concrete mechatronic components for a humanoid robot called 
ARMAR (see figure 1) that can share its working space with humans. 



Fig. 1. Upper body of the humanoid robot ARMAR III. 
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The long-term target is the interactive work of robots and humans to jointly accomplish 
specified tasks. For instance, a simple task like putting dishes into a dishwasher requires 
sophisticated skills in cognition and the manipulation of objects. Communication between 
robots and humans should be possible in different ways, including speech, touch, and 
gestures, thus allowing humans to interact with the robots easily and intuitively. As this is 
the main focus of the collaborative research centre, a humanoid upper body on a holonomic 
platform for locomotion has been developed. It is planned to increase the mobility of 
ARMAR by replacing the platform with legs within the next years, which will lead to 
modifications of the upper body. 

1.2 State of the Art and Motivation 

The focus of this paper is the design and the development process of a new wrist for the 
humanoid robot ARMAR. The wrist serves as the connection between forearm and hand. 
An implementation of the new modules is planned for the next generations of the humanoid 
robot, ARMAR IV and V. The wrist of the current version, ARMAR III, has two degrees of 
freedom (Albers et al., 2006) and its rotational axes intersect in one point. ARMAR III has the 
ability to move the wrist to the side (± 60°, adduction/ abduction) as well as up and down (± 
30°, flexion/ extension). This is realized by a universal joint in a compact construction. At the 
support structure of the forearm all motors for both degrees of freedom are fixed. The gear 
ratio is obtained by a ball screw in conjunction with either a timing belt or a cable. The load 
transmission is almost free from backlash. The velocity control and the angular 
measurement in the wrist are realized by encoders at the motors and by quasi-absolute 
angular sensors directly at the joint. To measure the load on the hand, a 6-axis force and 
torque sensor is fitted between the wrist and the hand. 

One of the main points of criticism on the current version of the wrist is the offset between 
the rotational axes and the flange, as shown in figure 2 (left). Due to the joint design, this 
offset distance is necessary in order to provide the desired range of motion. Also other 
wrists of humanoid robots show a similar design, see (Shadow), (Kaneko et al., 2004), (Park 
et al., 2005), (Kaneko et al., 2008). That offset is even greater due to the placement of the 6- 
axis force and torque sensor. The resulting movement, a circular path performed by the 
carpus, does not appear as a humanlike motion, as illustrated in figure 2 (right). 




Fig. 2. Offset between the rotational axis and the hand flange at the wrist of the humanoid 
robot ARMAR III (left) and the resulting movement (right) 
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The German Aerospace Centre DLR (Deutsches Zentrum filr Luf t- und Raumf ahrt) has been 
working on seven degree of freedom robot arms for several years. The result of this project 
is shown in figure 3 (left). Although their work is inspired by a human arm, their goal is not 
to design humanoid robots. The wrists of the lightweight arms of the third generation 
imitate human wrist movements by a pitch-pitch combination with intersecting axes 
(kardanic). An alternative pitch-roll configuration is also utilized, mainly for applications 
using tools (Albu-Schaffer et al., 2007). Both versions have an offset comparable to the 
current wrist of ARMAR III. 

Henry J. Taylor and Philip N.P. Ibbotson designed a so called "Powered Wrist Joint" 
(Rosheim, 1989) in order to load and unload space shuttles. The concept of this wrist is 
illustrated in figure 3 (right). In a smaller version, the basic idea could be reused in 
humanoid robot's wrist. The second degree of freedom (pitch) of the wrist is guided by a 
spherical joint. Such an assembly provides a slim design and relatively wide range of 
motion. The actuators for the second degree of freedom (yaw) are located directly at the 
joint; therefore, the drive units are quite simple. On the other hand, miniaturization seems to 
be very difficult due to the dimensions of common gears and motors. 




Fig. 3. The DLR/Kuka lightweight robot arm (Abu-Schafer et al. 2007) (left) and concept for 
a wrist actuator (Rosheim, 1989) (right). 



2. New Concept 

2.1 Requirements and Design Goals 

In this section the system of objectives is defined. It describes all relevant objectives, their 
dependence and boundary conditions, which are necessary for the development of the 
correct object system, outgoing from the current condition to the future condition. But the 
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solution itself is no part of the system of objectives. It is permanently extended and 
concretized over the complete product lifecycle. The correct, consistently and complete 
definition of this system is the basis of the successful product development and a core 
component of the development activity (Albers et al., 2008a). Since the robot is intended to 
get in contact with humans in order to achieve various functions, it is inevitable that the 
robot is accepted by the human. The ability to move like a human is as important as a 
human-like appearance; therefore, specific demands (Asfour, 2003) on kinematics, dynamics 
and the design space must to be considered. A human wrist consists of many different 
elements and has a relatively wide range of motions. Figure 4 illustrates the different 
possible movements of the human wrist along with the corresponding reachable angular 
position of the joints (Whired, 2001). 




Fig. 4. Human wrist and range of motion: a = palmar flexion 70°, b 
radial abduction 20°, d = ulnar abduction 40° (Whired, 2001). 



dorsal flexion 90°, c 



In order to implement a human-like wrist movement, two orthogonally arranged rotational 
degrees of freedom are necessary. Both axes are orthogonal to the forearm's axis and 
intersect in one point. The two degrees of freedom need to be put in a kinematical series. 
The requirements and design goals for a humanoid robot's wrist can be deduced based on 
the range of motion of the human wrist. The first degree of freedom should have a ±30° 
range of motion and the second about ±90°. The wrist will be attached to the forearm's 
structure on one side and provides the connection to the hand. It should be possible to 
disconnect the mechanical joint between the hand and wrist in a simple way in order to 
enable a modular design. To measure the load on the hand, a 6-axis force and torque sensor 
must be fitted between the wrist and the hand. The electronic cables and pneumatic tubes 
supplying power to the hand actuators are the similar to those used in the previous models 
of ARMAR (Schulz, 2003; Beck et al., 2003). The design space for the robot's wrist is based 
on human dimensions as far as possible; therefore, one aim is to keep a sphere of 
approximately 100 mm in diameter as a boundary. At the same time, the control strategy 
aims to operate all degrees of freedom as individually as possible. 

In keeping with the standardized drive concept of most modules of the robot, electronic 
motors are used as the source for actuation. The drive units need to be dimensioned for a 
load of 3 kg. All gears are designed to be free from backlash and not self-locking. But 
friction, e.g. in case of a loss of power, leads to a slow and damped sinking of the arm 
instead of abrupt movement. That is of great importance for an interactive application of the 
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robot in a human environment. On the other hand, stick-slip effects in the gears have been 
avoided, which is a clear benefit for the control system. 

Finally, the mechanical structures should be as light as possible in order to save energy 
during dynamic movements. A lower mass of the wrist can contribute significantly to a 
reduced energy consumption of the whole arm and has a strong influence on the gears and 
motors used for the drive units for the elbow and shoulder degrees of freedom. 

2.2 Concepts 

A simple reduction of the wrist's length by only minor modifications is not possible. This is 
mainly because the current joint design in combination with the drive unit for the second 
degree of freedom does not allow a mounting of the hand in the rotational axis. Formulated 
in an abstract way, the development goal is to shift material from the intersection point to a 
different location in order to gain free space in the centre position. 

Bodies in general have six degrees of freedom in a three dimensional space: three rotational, 
and three translational. Due to design complexity, the degrees of freedom must be reduced 
for the development of a technical joint. As technical solutions in robotics usually have only 
one degree of freedom, it is necessary to combine two basic joints to implement a two degree 
of freedom joint (Brudniok 2007). An alternative solution is a spherical joint where one 
rotation is blocked, but actuators for such a design have not yet been sufficiently developed. 
As result of these basic considerations, two principle solutions were found: a universal joint 
and a kind of curved track as depicted in figure 5. 















tnzzzzj- E <"**« \l 


] 


A 1 

1/ 


/ 
1 








Di^/ 





Fig. 5. Universal joint (left) and the principle curved track solution (right). 



To illustrate the decision process within the development both concepts are discussed 
shortly. The universal joint concept (figure 5 left) is very similar to the current solution 
running on ARMAR III. The first degree of freedom is provided by a rectangular frame (A). 
On that frame there is enough space for the bearings (B) of the second degree of freedom. 
Finally, the hand can be mounted on the plate (C). In contrast to the current version, the 
reduced length was achieved by taking all elements in one plane. The disadvantage is that 
the outer diameter has to be enlarged in order to provide the wide range of motion 
described in the previous section. One possible implementation of the drive units could be a 
direct connection by bowden cables providing a slim and light design of the joint itself. By 
applying this idea to the universal joint, the total length (TL) of each cable changes. Figure 6 
illustrates the parameters which are of importance for a two dimensional consideration. 
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Fig. 6. "Changing" length of the cables in different angular positions of the wrist. 



The total lenght can easily be calculated by the following formula, where a denotes the 
angle between the cables and the middle axis of the forearm: 



TL = a + b = 2- 



I 



cos(a) 



(i) 



As a depends on the angular position of the wrist, TL changes during each movement. That 
means that the different degrees of freedom can not be run independently as long as 
electronic motors are used as actuators. The Shadow Hand, for example, uses a different 
concept concerning the cables and their changing lengths (Shadow). 

The second basic concept depicted in figure 5 on the right side consists of a curved track 
solution for the first degree of freedom (D). As this first rotation is limited to ±30°, there is 
enough space left for the bearings of the second degree of freedom, which may be realized, 
e.g., by a simple shaft (E). This configuration allows a relatively wide range of motion and a 
high capability for a reduction of the wrist's length. The challenges for this concept include 
finding a technical solution for the curved track, a suitable actuation and a design with a 
proper stiffness in the structures. 

Overall, both basic concepts fulfill the principle requirement of length reduction. The curved 
track method, however, has a clear advantage in terms of size in the radial direction. The 
oval outer contour also shows a better similarity to a human wrist; therefore, the curved 
track concept was selected for further development. 



2.3 Embodiment Design 

By an appropriate design of the shaft (see figure 5 right, named E) it is possible to gain still 
more space for the 6-axis force and torque sensor. Figure 7 illustrates a cross-section view of 
the modified shaft. The depth of the shell corresponds with the radius of the curved track 
and enables a mounting of the hand exactly in the point where the rotational axes intersect. 
This is achieved by shifting the mechanical connection in the negative direction along the 
center axis of the forearm. 
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Fig. 7. Basic idea for the shaft of the second DOF of the wrist integrating the force sensor. 

For the technical implementation of the curved track, a curved guide named HCR 
manufactured by THK was selected. Used for medical applications, THK produces ceramic 
curved guides with a radius of approximately 100 mm. From a technical standpoint it would 
have been possible to reduce the radius to meet the requirements for a humanoid robot's 
wrist. For economic reasons, however, this was not a feasible option for the collaborative 
research centre. Therefore, a different solution was necessary. 

The curved guide was replaced by rollers in combination with a timing belt. This allowed 
the integration of two different functions in one element: the timing belt functions as part of 
the drive unit while also providing sufficient pre-load to avoid a gap between the rollers 
and the track. Figure 8 shows the basic CAD model of each design. 




Fig. 8. First technical solution by using a curved guide (left) and the alternative using a roll 
timing belt combination (right). 



3. Simulation 

3.1 Basic Geometric Considerations 

Based on the new concept an analytic model can be set up. Therefore all geometrical 
parameters based on the nature of the human body have to be adapted to the model. The 
undefined variables have to be calculated and estimated using the analytical model to get a 
reasonable set of values for the design. Using parameter optimization the best combination 
of values for a design proposal can be found in order to achieve reasonable preloads for the 
belt. 

Two main load cases were used whereas the angle of the initiated force cp can vary. Figure 9 
illustrates these two load cases. Here the calculated force F (36 N) is the substitute for all 
external loads and self -weight (Albers et al., 2006). M is the appropriate torque resulting 
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from the arm of lever and is about 3.14 Nm. To avoid a displacement of the cap the preload 
Fv has to be chosen great enough 




Fig. 9. Load case I (left) and load case II (right) in two different directions. 



Load case I: 

The external load F is applied in a variable angle cp towards the vertical line. The maximum 
required preload for an offset of the beveled wheels (d) of 37.5 mm is about 1 kN. When d is 
increased to 42.5 mm, the required force is less than 0.63 kN. Thus, the required force 
decreases by about 37 % when the off -set of the beveled wheels is increased by about 21 %. 
By doubling the distance from 35 mm to 70 mm, the required preload force is reduced by 
90 %. The calculated critical angle of the load cp is 36°. 

Load case II: 

Calculations have shown that the influence of the substituted shear force F is negligible for 
this load case. Therefore only the over-all torque M is used for the analysis. Fv is dependent 
upon the angle c, and the wheel distance e. The calculated maximum force for the timing belt 
is 0.28 kN. The calculated forces are all in a reasonable range compared to the technical 
elements that can be used for the construction. Consequently the concept can be realized in a 
physical system with standard bearings and materials. 

3.2 First Design and Finite Element Analysis 

On the basis of the analytic results described in section 3.1, the optimal solution for the free 
geometrical parameters can be defined and in a further step be designed in a CAD system. 
An impression of the parameter optimized wrist is given in figure 10: 




Fig. 10. CAD model based on the results of the analytical considerations. 
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In a next step the CAD model is simulated numerically using Finite Element Method (FEM) 
in order to gain further information of the system's behavior. Especially the elasticity of the 
different structures and the resulting interaction effects are of interest. The preload force and 
the orientation of the external force were varied systematically. The primary object is to get 
values for the displacement of the cap towards the global coordinate system. ABAQUS 
(Dassault Systemes) is the used solver for the FEM. In order to reduce the computing time, 
the CAD model must be simplified while the fundamental behavior of the system should be 
modeled as accurately as possible. The following parts are taken into account for the Finite 
Element Analysis (FEA): The cap, the beveled wheels, the idler and the timing belt are 
modeled as deformable with the ABAQUS- element type 'C3D8I' (except beveled wheels, 
C3D8R). Analytical rigid elements are used for the connecting wheels and the driving shaft. 
All deformable parts are simulated with isotropic material except the timing belt. Due to the 
fact that the timing belt is composed of a steel cord with polyurethane backing and teethes, 
an anisotropic material parameter is used in the model. The angle cp of the external load 
takes the value of 0° and 36° which is identified in the analytic calculation as the most 
critical. Figure 11 illustrates the result of the FEA. 



S, Mises 

(Ave. Crit. : 73*) 
I — ^+9.947e+01 
^S- +2 .000e+01 

P^f- +1 . *: j je + 01 
W- +1 .667e+01 




Fig. 11. Stress distribution (von-Mises criterion) for load case II. 



The stresses, obtained by the FEA show a reasonable distribution. The displacement of the 
cap tested with high preload forces is minimal due to the FEA. For a preload of 0.6 kN the 
displacement for load case II is about 4.4-10 -3 mm and for load case I 1.39T0 -2 mm. 
Compared with a preload of 1 kN the displacement doesn't highly decrease. For load case II 
the displacement takes the value of about 4.07-1O 3 mm and for load case II 1.29-10 2 mm. 
These values for the different preloads show that in the range between Fv=0.6 and 1.0 kN 
only a small increase of positioning accuracy due to less displacement can be reached. But 
the high additional costs in the construction of the wrist for preloads higher than 0.6 kN 
can't be justified. For this reason, and for practical implementation, it is not meaningful to 
use forces greater than 0.6 kN. For preloads lower than 0.25 kN the position deviation 
increases dramatically and the system becomes statically indeterminate. The displacement 
of load case I with cp=36° is for every point smallest compared with load case I (cp=0°) and 
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load case II. Therefore, it appears that a preload between 0.25 kN and 0.6 kN would be most 
suitable. 



4. Functional Prototype 

Based on the positive results obtained by the different simulations, a functional prototype 
was developed. That was necessary mainly because different functions were integrated in 
the toothed belt, which is usually used in a different manner and not all material parameters 
were available so that estimated values were used. 

As the purpose of the prototype is to prove basic functionality of the design, a few 
simplifications are made. For the beveled wheels complete rolls are used and the cap is 
designed in a simple way for instance. Further-more, the construction allows the possibility 
to implement an s-beam force sensor (Lorenz K-25). Figure 12 shows two pictures of the 
assembled functional prototype with a one kilogram weight attached at the hand's position. 




Fig. 12. Functional prototype. 

Multiple static and dynamic tests show that this configuration is very accurate and has a 
high stiffness for small preloads of about 300 N. Hereby the wrist is hand-held at the 
forearm tube and statically loaded by huge forces between 20-80 N or moved dynamically in 
all different directions. Even for very fast "hand actuated" motions, which were 
approximately five times of the maximum velocity of the robot's arm, the assembly 
remained free from backlash. 



5. Optimization and Lightweight Design 

As a lightweight design is one of the main goals for the development of the new wrist, 
different numerical optimization methods were used. 



5.1 Topology Optimization 

Topology optimization is used for the determination of the basic layout of a new design. It 
involves the determination of features such as the number, location and shape of holes, and 
the connectivity of the domain. A new design is determined based upon the design space 
available, the loads, possible bearings, and materials of which the component is to be 
composed. Today topology optimization is very well theoretically studied (Bendsoe & 
Sigmund, 2003) and also a common tool in the industrial design process (Pedersen & 
Allinger, 2005). The designs, obtained using topology optimization are considered as design 
proposals. These topology optimized designs can often be rather different compared to 
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designs obtained with a trial and error design process or designs obtained from 
improvements of existing layouts. The standard formulation in topology optimization is 
often to minimize the compliance corresponding to maximize the stiffness using a mass 
constraint for a given amount of material. That means that for a predefined amount of mass 
the structure with the highest stiffness is determined. Compliance optimization is based 
upon static structural analyses, modal analyses or even non-linear problems, such as models 
including contacts. A topology optimization scheme is basically an iterative process that 
integrates a finite element solver and an optimization module. Based on a design response 
supplied by the FE solver (e.g. strain energy), the topology optimization module modifies 
the FE model. 



5.2 Material Optimization 

Besides the topology optimization, it is necessary in addition to consider optimization 
strategies such as material optimization. Extreme lightweight design is possible only by 
combining both optimization strategies such as the topology optimization in combination 
with an optimal fiber layout. For calculation of laminates by use of the Finite Element 
Method (FEM), approaches are used that combine the properties of single plies to one 
virtual material by use of the 'Classical Lamination Theory' (CLT) (Johns, 1999). These 
established theories are valid for the elastic range. 

Several approaches for the determination of optimal fiber orientation have been presented in 
the past. (Luo & Gea, 1998) use an energy based method. (Setoodeh, 2005) describes an 
optimality criteria approach, while (Jansson, 2007) works with a generic algorithm. Inspired 
by nature (Kriechbaum 1994), (Hyer & Charette, 1987) place fibres in direction of first 
principal stress. In that context (Lederman, 2003) presents a method placing the fibers in the 
direction of the first main stress in the finite element. (Pedersen, 1991) showed, that a fiber 
orientation according to the first main strains leads to maximization of stiffness. Most of 
those approaches only work for one layer, and are reduced on two dimensional problems. 
The method used in that work was developed by (Albers et al., 2008b), focusing two main 
goals: Fast convergence, because the approach is intended to be used together with FEM, 
and, in a second step, combination with topology optimization. Application should be 
possible for 3D-geometries, and determination of a two layered laminate structure 
(orientation and thicknesses) had to be possible to take multi-axial load cases into account. 
The approach is based on a theory described by (Ledermann, 2003). Optimal fiber 
orientation is found, if it is equal to the orientation of the first main stress. To be able to take 
multi-axial load cases into account, the method creates two plies per finite element, with the 
second ply oriented in the direction of the second main stress. The relation of thickness of 
the two plies is proportional to the relation of the two main stresses. The orientation of the 
composite in space is defined by the surface created by the two directions of the main 
stresses. The third main stress is not taken into account, because 3-dimensional canvases are 
normally not used in real world applications. 

The method is implemented in an iterative procedure, starting with a finite element model 
with isotropic material. Thenceforward, the isotropic material model is replaced by an 
anisotropic one with the parameters of a combined two-layer composite. Stress and ply 
directions are updated in every iteration. In detail, the following steps are undertaken in 
each iteration: From the preceding finite element analysis, main stress directions and - 
amounts are determined for each finite element. The procedure starts with the 
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transformation of the direction vectors of main stresses from the element coordinate systems 
to the vector of the global system by use of the direction cosines. The cross product of the 
direction vectors of the two first main stresses is used to define the perpendicular to the later 
surface of lamina of the element. In the special case of the cross product being the zero- 
vector, e.g. the uniaxial stress condition, a filter is used to determine the perpendicular out 
of the neighboring elements. By use of the given engineering constants E||, E-L, uJ-||, u J-l and 
G-L|| of the chosen fiber-matrix-combination, the orthotropic stiffness matrix [C] of the UD- 
layers can be reduced to a transversal isotropic one as follows: 
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The angle a between the two layers is the angle between the two first main stresses. It can be 
obtained by use of the direction cosines. The volume share of the two layers is calculated as: 



<x, + \cr z \ 



,o 2 =\-o x 



(6) 



By use of the 'Classical Lamination Theory' (CLT), the combined stiffness matrix [C com ] of 
the two-layer-lamina can now be calculated. First, the stiffness matrix of the smaller layer is 
transformed into the lamina coordinate system, defined by the direction of the first main 
stress. This is done by rotating the stiffness matrix of the layer [C]' about a: 



[c] = [tY[c][t\ t 



(7) 



With [T] the following transformation matrix: 
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The combination of the two layers is done by use of the rules defined by the CLT. The 
iteration is finished by formatting and writing the new anisotropic stiffness matrices in the 
input deck for the FEA. Depending on the FE code used, the materials has to be filtered and 
clustered before a FEA can be performed, as some FE algorithms are limited in the number 
of materials allowed. Reduced convergence speed and accuracy of the approach may result. 



5.3 Model Setup 

For the topology and material optimization the complete system is disassembled and only 
the cap is used for the optimization. This simplification is necessary to avoid enormous 
computing time caused by a very fine mesh for the cap and a huge number of load cases. 
Cutting these parts free from the total system, calls for a realistic replacement of the 
interaction between the components. Hereby the interaction between the beveled wheels 
and cap is replaced by a connection at the corresponding nodes which allows a degree of 
freedom in the 3-axis direction. This simplification is possible because the appearing forces 
can only be compressive force or the cap lifts off the wheel surface. The timing belt is 
replaced by a load which is tangential to the cap and transferred to the structure by 5 points 
on each side of the cap. The preload used in place of the timing belt is 450 N. On one side of 
the cap an additional force is applied to the timing belt which is the result of an inertial relief 
and named Fmertrei- The external load (F) is defined to 30 N and applied to the cap by a 
torsion arm, which is modeled as rigid element (RBE in MSC.Nastran), in a distance of 
100 mm in negative 3-coordinate-axis. The force vector can be reduced to three different 
directions because of the symmetry conditions can be defined for the optimization process. 
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The additional torque (M) represents 5 Nm and rotates about the global 3-coodrinate-axis. In 
figure 13 eight different load cases are illustrated. 
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Fig. 13. Load cases for the topology optimization. 

The first six load case combinations are set for three different rotational positions of the 
second DOF. The initial state is the neutral position and two other positions are realized by 
changing the direction and position of all forces on the cap as they would appear at a 
±30° rotation. From this it follows that 18+2=20 different configurations of the cap are set for 
the topology optimization. 



5.4 Results 

The result of the topology optimization as a basic design proposal is shown in figure 14. The 
complex topography in the center is a result of the stress caused by the torsional moment 
applied to the structure mainly by load case 4 (4.1, 4.2, 4.3). The direct connection to the 
bearing by the beveled wheels is visible clearly. The function of the center link is mainly to 
absorb the reaction forces applied to the cap by the high preload and the beveled wheels. 
The side links are important to reinforce the cap structure between the two points of force 
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transmission of the timing belt. The shape of an arrowhead as a result of the two side links is 
highlighted on the right side in figure 14. 



center link 



side link 




side link 



zO 



center 




Fig. 14. Smoothed design proposal as a results by the optimization for a lightweight 
laminate. 

In figure 15 the principle stress for an anisotropic topology optimization is illustrated. The 
highlighted regions are areas with preferred orientated principle stress, which is important 
while using fiber reinforced materials. Furthermore the stress is uniaxial in these regions. A 
zoomed view (round clippings) clarifies the stress orientation. In these regions the fibers in 
the laminate can be orientated in the direction of the principle stress which on the one hand 
reduces the amount of used material and by that the weight of the cap and on the other 
hand it improves the stiffness and accuracy. Regions which are not highlighted have 
changing stress orientations and different stress states. In these areas laminates have to be 
stacked with different orientations to absorb the multiaxial stress. 




Fig. 15. Principle stress highlighted for a laminate. 
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Based on the optimization results a 3D-CAD model can be implemented as a first design 
proposal (see figure 16). In order to realize a lightweight design, the part is built up as an 
hollow shell. The advantage is that the material is located at the outer area which increases 
the bending stiffness. To reduce the comprehensive stress at the friction contact zone where 
the beveled wheels are crawling, a metal band is laminated into the fiber composite. This 
metal band also increases the stiffness. Furthermore compression proof foam can be 
integrated into the shell in areas with high pressure mainly introduced to the structure by 
high preloads. The complex suggestion for the center by the topology optimization is 
reduced to a thickened center link. To big holes in the structure reduce the weight. In 
figure 16 on the right side the arrowhead shape, which was suggested by the optimization, 
is visible. This shape allows a good distribution of forces within the structure. 




/ friction contact zone 
for the bevel wheels 




holes 



arrowhead shape 



Fig. 16. 3D-CAD model of a design proposal. 

Due to the two holes in the structure a new concept for the timing belt is required. Therefore 
two narrow belts instead of one are used to apply the preload to the cap. In figure 17 a 3D- 
CAD model with a suggestion for the two timing belts is illustrated. This arrangement 
increases the support effect and results in a better positioning accuracy. 




Fig. 17. 3D-CAD model of a design proposal for the timing belt. 
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6. Conclusion 

In this paper the development of a new concept for humanoid robot's lightweight wrist is 
presented. Especially the different steps of the development process are described. Based on 
the basic ideas, different analyses and simulations are conducted. A functional prototype is 
presented which is a kind of proof of the concept. Due to the design proposal obtained by 
the topology optimization for a fiber composite, a lightweight design is implemented in the 
CAD model. 

The next step will be the integration of the drive units for the second degree of freedom. 
Here different solutions are possible, e.g. like bowden cables or a direct actuation in 
combination with harmonic drive gears. In order to achieve a further reduction of mass, 
composite materials may be used for some further of the structural components. The new 
wrist will be developed in the next months and is to be manufactured and assembled during 
the next year. 
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1. Introduction 

Dexterous robot hand development is a very challenging and interesting research topic. A 
dexterous robot hand may serve as a prosthesis hand for disabled patients or to serve as a 
gripping device for robotic arms. In most of previous studies, the dexterous robot hands are 
developed based on the directed gear train controls and tendon wired controls. The directed 
gear train control based design (Lin et al., 1996; Namiki et al., 2003) directly coupled the gear 
train in the finger module mechanisms. In such a configuration, the weight of the dexterous 
robot hand is quite heavy because of using numerous gear parts and motors. At the same 
time, the mechanical design and the assembly of the directed gear train dexterous robot 
hand are much complicated. Meanwhile, the heats resulted from high reduction of gear 
trains as well as the high speed rotation of motors are also challenging issues of directed 
gear train based dexterous robot hands. Consequently, the weights and heats are major 
concerns when applying this configuration to the prosthesis hand for amputees. 
On the other hands, the tendon wire control based dexterous robot hand allocates the gear 
trains and motors at a distance location (Jacobsen et al., 1986; Kyriakopoulos et al., 1997; 
Challoo et al, 1994). In this manner, the weights and heats produced from motors and gear 
trains are resolved when compared to the directed gear train based configuration. 
Nevertheless, the non-rigid characteristics and frictions of the tendon wires are also 
important to the precise control of a dexterous robot hands. 

In this chapter, a dexterous robot hand with tendon wired control is proposed to perform 
the characteristics of compact size and low weight. According to this purpose, the dexterous 
robot hand size is defined as the hand size of a twenties male. In order to reduce the weight 
of the dexterous robot hand, the ABS engineering plastic material is used in this study. 
Additionally, to emulate the hand motions of a human, the dexterous robot hand is 
designed as a five-finger mechanical structure. Consequently, the proposed dexterous robot 
hand is composed of 16 joint motions. To reduce the control complexity, 12 active joints are 
independently controlled; and the remaining four joints are manipulated depending on four 
corresponding active joints. 

At the same time, five FSR pressure sensors are attached on the tips of all fingers to detect 
external forces applied on the corresponding fingers. Therefore, the robot hand is capable of 
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gripping objects with different griping force setting. In addition to the mechanical design 
and sensor deployments, the motion trajectories for different griping behaviors (Parka et al., 
2006) are also constructed. These motion trajectories are desired to synchronously control 
the finger joints for achieving various griping behaviors. Consequently, the tactile force 
sensing (Kawasaki et al., 2007; Kawasaki et al., 2002) may combine with the motion 
trajectories to perform force feedback based griping control systems. Finally, the proposed 
dexterous robot hand prototype is developed to demonstrate its motion behaviors. 

2. System architecture 

In this section, the system architecture is described. The proposed system architecture is 
composed of the mechanical design, behaviour based gripping motion planning with tactile 
force sensing, and system integrations, as shown in Fig. 1. The mechanical design 
specifications include the size and skeleton of robot hand, degree-of-freedom, range of 
motions of each finger, and orientation of each finger. These specifications are defined 
according to the hand profile and various gripping postures of a twenties male student in 
our laboratory. Therefore, the proposed dexterous robot hand may demonstrate similar 
motions with the human beings. In addition, a tendon wired control configuration is 
proposed in this study to reduce the weight and heat of the hand. Especially, the ABS 
engineering plastic material is used to further reduce the weight of the robot hand. 



Mechanical 
Design 



Design Specifications 



Tendon Wired Actuations 



Gripping 
Motion 
Planning 



Gripping Behavior Modeling 



Tactile Sensing 



Gripping Trajectory Planning 



System 

Integration/ 

Test 



Motion Control System 



Parameter Setting and 
Training System 



Fig. 1. System development architecture 



The gripping motion planning module acts the supervisory controller of the proposed 
dexterous robot hand. It is responsible of modelling gripping motions in terms of setting the 
initial and final postures of fingers, acquiring the tactile sensor data from the tips of all 
fingers, and planning the gripping trajectory based on the gripping models, maximum 
allowable tactile forces and maximum allowable joint angles. Note that the gripping motion 
planning module may just model simple griping motions in the current stage. Finally, the 
system integration and test module is desired to cooperate with the motor controller to 
manipulate the robot hand. At the same time, the operation parameters such as the 
maximum tactile force and grip model selection can be also desired. 
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3. Mechanical design 

In this study, a dexterous robot hand is proposed based on the design concepts of light 
weight, compact size, similar to human's hand gripping motions, gripping force restriction, 
and low cost. In order to meet these design concepts, the following design issues and 
directions are discussed before this research project. 

1. Light weight: Most of dexterous robot hands are designed based on directed gear train 
controls and tendon wired controls. Because of the distance motor and gear train 
configurations of the tendon wired control robot hand, the weight of the robot hand can 
be reduced. At the same time, an ABS engineering plastic material is also used to 
fabricate the hand to further reduce the weight of the robot hand. In this manner, the 
light robot hand may reduce the loads of the robot arms for service robots as well as the 
loads of the upper extremity for hand amputees. 

2. Compact size: One of the study purposes of this dexterous robot hand is to produce a 
prosthesis hand for hand amputees preliminarily. Therefore, in addition to the robot 
hand weight reductions, the hand size and profile must be similar to the human beings. 
In this study, the proposed dexterous robot hand is designed referring to a twenties 
male student in our laboratory. Consequently, a five-finger robot hand with 16 degree- 
of -freedom is presented. 

3. Emulating human's hand gripping motions: In addition to similar hand structures, the 
range of motions of human's hand is also evaluated. The range of angles of the joint is 
defined via physically measuring the angle of finger joints. In addition, several gripping 
motions such as gripping apples, eggs and pens are simulated using the computer 
aided design (CAD) software. 

4. Gripping force restriction: Gripping force restriction is important to the robot hand. The 
maximum force restriction may provide a sufficient gripping force when the finger tip 
touches the objects, but the force will not damage the gripped objects. At the same time, 
the maximum allowable tactile force may also protect the wire and motors of the robot 
hand. 

5. Low cost: In general, the cost of a dexterous robot hand is quite expensive because of 
using high performance DC/ AC servo motors. In recent years, the advance RC (radio 
servo) techniques provide a simple and low cost position servo control solution. 
Therefore, the RC servo motors are used in this work to reduce the cost of motors. 

6. Other concerns: In addition to the previous considerations, the tendon wired control 
robot hand may also eliminate the heats resulted from the motors and gear trains when 
compared to the directed gear train control robot hands. At the same time, the distance 
actuated robot hand can be applied in more strict environments such as water. Finally, 
to reduce the control complexity, the joint motions of distal interphalangeal joints of the 
index finger, middle finger, ring finder and little finger are designed to be dependent 
on the joint motions of the proximal interphalangeal joints of the corresponding fingers. 
As a consequence, the motor number is reduced as 12 in this study. 

Mechanical design of the proposed dexterous robot hand uses the Pro/E CAD software tool. 
As described before, the robot hand profile and structure is referred to the hand of a 
twenties male student in our laboratory. Fig. 2 shows the hand photo of the volunteer. 
Design parameters of this robot hand follow the hand structure of this hand profile. In order 
to increase the producing efficiency of mechanical parts, geometries of these mechanical 
parts are modified and designed as several uniform specifications, as shown in the right- 
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hand-side of Fig. 2. Note that the little finger just designed as a two-phalanx structure 
because of infrequent uses of this finger as well as reduction in the length of this finger 
when these uniform mechanical parts are used. 
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Fig. 2. Hand photo of volunteer and design parameter of robot hand mechanical structure 
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Fig. 3. Mechanical structure design and ranges of joint angles of the proposed robot hand 



In addition to design a similar structure with human being, range of joint motions are also 
discussed in this study. The ranges of joint motions of fourteen volunteers are evaluated as 
shown in the right-hand-side of Fig. 3. The joint symbols are referred to the left-hand-side 
CAD model. These parameters are mean- values measured from fourteen volunteers. 
Especially, to reduce the control complexity, the joint motions of distal interphalangeal 
joints and proximal interphalangeal joints of A, D, G and I (referred to Fig. 3) are designed 
as dependently actuated. The joint angles of distal and proximal joints are correlated in 
terms of the ranges of joint motions. In practice, different diameters of pulleys and cable 
wires are used to produce synchronous actuations of distal and proximal joint within the 
defined angle ranges. Fig. 4 shows the design details. 

The mechanical parts of this robot hand are produced in the machining shop of our 
university. Bearings are used in all rotary parts to reduce the frictions, as shown in the left- 
hand-side of Fig. 5. In addition, the tactile sensor socket is also desired at the distal phalanx 
part of each finger, as shown in the right-hand-side of Fig. 5. 
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Fig. 4. Design details of a finger CAD model 



In order to reduce the weights of mechanical parts, the ABS engineering plastic material is 
used, and all mechanical parts are shown in Fig. 6. 




Fig. 5. Parts with bearings and tactile sensor socket 




Fig. 6. Photos of produced mechanical parts and assembly of the hand 



Because of referring the hand profile of a twenties volunteer, the size of the fingers are 
evaluated as shown in Fig. 7. In this figure, the index finger of the volunteer are compared 
with the robot hand. Apparently, they are in a similar finger profile. Finally, because of 
using the ABS engineering plastic material, the weight of the hand can be reduced. The 
weight for each finger is summarized in Table 1. To investigate the mechanism performance, 
several hand postures of human beings are simulated using the 3D CAD tool, as shown in 
Fig. 8. As a consequence, these hand postures can be properly desired using the proposed 
mechanical structure of this robot hand. 
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Fig. 7. Finger profile comparisons for a twenties volunteer and the proposed robot hand 

Unit: gram 
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Table 1. Weights of fingers of the robot hand 

Finally, the tendon wired control configuration is introduced. In this study, a multi-cord 
steel wire with 0.8 mm diameter is used. In addition, the tendon wire is surrounded with a 
spring cord as shown in Fig. 9. I addition, the assembled tendon wired control robot hand is 
also presented in Fig. 10. The photo of the wire actuated robot hand is shown in the left- 
hand-side of Fig. 10; and the photo of the distance motor site is shown in the right-hand-side 
of Fig. 10. All RC servos are mounted at the motor platform, and it can be installed off the 
hand and arm so that the load of the arm can be reduced. 




Fig. 8. 3D CAD hand posture simulations 
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For example, the motor platform can be installed inside the body of a wheeled robot or a 
humanoid robot to increase the carry loads of the arm and hand. Note that the motor 
platform is just installed under the robot hand for demonstrations in this chapter. 



Sprin 
Cord 




RC Servo Motors 

Fig. 9. Tendon wired control configuration 




Fig. 10. Photos of wire actuated robot hand (robot hand site and distance motor site) 



4. Behavior based gripping motion planning with tactile force sensing 

In this study, the developments of a dexterous robot hand do not focus on the mechanical 
structure design, but also on the gripping behavior modelling. In order to simplify the 
gripping model as well as to perform more realistic approach, only simple and frequent 
gripping behaviors are discussed such as gripping an apple, cylinder, egg, etc. It is noted 
that the precise position controls of all finger tips using inverse kinematics are not the focus 
of this study due to the difficulties of getting a precise spatial position and orientation of the 
gripped object in this study. Instead, this study investigates the joint motions from an initial 
posture to a final posture of a hand for each interested gripping motion. Fig. 11 shows the 
initial and final postures of a robot hand for gripping an egg and an apple, respectively. 
All joint angles of the initial hand and final postures are recorded. The gripping motion can 
be synchronously desired according to the interpolations of the joint angles of the initial and 
final postures. Therefore, the gripping model is formed based on the initial and final joint 
angles as the synchronous interpolations of these joint angles. 
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Initial Posture 




Fig. 11. Gripping postures simulations of an apple and an egg 



Most of initial postures are the same (complete extraction of all fingers, as shown in the top 
figure of Fig. 11). However, the final postures are quite different for various gripping 
models. Equation (1) shows the gripping model for a specific gripping motion. 



T 

AT 



A0 k = 



®k,f ~ &k,i 



k (t) = k , + tA0 k 



(1) 

(2) 
(3) 



where T is time required for the gripping motion; T is angular position command time 
interval for joint motors; n is theoretical count of position commands for a specific gripping 
motion; k,i is the initial joint angle for joint motor k (k = 1 to 12); w-is the finial joint angle 
for joint motor k; / ( is the angle increasements of joint motor k; t is the index (from zero) 
of position commands. 

Especially, the gripping model just defines the synchronous joint angle increasements for all 
joint motors in each operation command with a pre-defined time interval. The actual final 
(stop) joint angles will not be identical to the pre-defined final posture because it is difficult 
to get the actual positions and orientations of the gripped object online. That means the real 
final postures will not refer to the pre-defined final postures; instead, they depend on the 
maximum allowable tactile sensor force as well as the maximum allowable joint angles to 
meet practical gripping situations. Hence, the actual operation commands for a whole 
gripping motion may smaller or greater than the theoretical count of operation commands, 
as shown in Fig. 12. Consequently, based on the tactile sensor data feedback and maximum 
allowable joint angle mechanisms, the final (stop) joint angles are determined finger-by- 
finger. 

Note that the tactile sensor used in this study is a conventional force sensing resistor (FSR). 
On the other hand, the maximum allowable joint angle is desired for the finger being not 
damage the gripped object during gripping. At the same time, the maximum allowable joint 
angle may also prevent the collisions and intersections of the hand mechanism for tactile 
sensor failures and gripping position uncertainties. 
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Fig. 12. Behavior-based gripping motions control architecture 

The algorithm of the proposed behavior-based gripping motion control system is described 
in Fig. 13. At the beginning of gripping operations, a gripping model is selected. The angle 
increasements of al joint angles are calculated according to the angular position (operation) 
command time interval and the time required for such a gripping motion. The operation 
command index (f) is set as zero at the startup. The joint angled are further updated to get 
closing to the object. The maximum allowable tactile forces and joint angles are examined 
finger-by-finger. 
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Fig. 13. Algorithm of behavior-based gripping motion control system 



The active finger is defined as a finger with neither the tactile force being below the 
maximum allowable force nor the joint angles belong to this finger being within the 
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maximum allowable joint angles. The system is running for all active fingers in terms of 
updating operation command index (t) as well as calculating new joint angle commands for 
active fingers. Finally, the gripping motion is completed when no active fingers existed in 
this system. 



5. System integrations and experiments 

The system integration and test module is desired to cooperate with the motor controller to 
manipulate the robot hand via implementing the behavior based gripping motion control 
algorithm. The control system of the proposed dexterous robot hand is constructed using 
the PSoC (Programmable System on a Chip) processor. The PSoC is a mixed-signal array 
microcontroller, and it is a product of Cypress Semiconductor. The core of PSoC is built 
based on a very-small Harvard architecture machine, named M8C In addition to the core of 
firmware, the most important feature is that PSoC provides reconfigurable integrated 
analog and digital peripherals. Therefore, the PSoC is very suitable to integrate the sensing 
and actuation devices of the dexterous robot hand. In addition, the analog multiplexer is 
also desired to extend the analog-to-digital channels of the FSR sensing. At the same time, 
the gripping models are stored in an EEPROM and the data is accessed via the I 2 C 
communications . 

In order to justify the tactile force variations among these five FSR sensors, the calibrations 
of FSR sensor are also desired in this study. At the same time, the operation parameters such 
as the grip model selection, maximum allowable tactile force for each finger, maximum 
allowable joint angles, theoretical operation time required for each gripping motion, and 
operation command time interval are downloaded from a host computer via RS 232 
communications. Fig. 14 shows the control circuit boards and the photo of gripping an egg. 
Ideally, only the fingers of thumb and index and middle fingers are used for gripping. 
Therefore, the terminations of the thumb, index and middle fingers should be done via 
checking the maximum allowable tactile forces. 



Ideal Gripping Model 




Fig. 14. Egg gripping experiment with inconsistent egg posture (middle finger cannot touch 
the egg, and this finger is terminated via maximum allowable joint angle of the egg gripping 
model) 
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However, due to inconsistent spatial position and posture of the egg, only the thumb and 
index can touch the tactile sensors in this experiment, and middle finger are terminated via 
the maximum allowable joint angle for this egg gripping model. Additionally, the 
terminations of the ring and little finger are also done in terms of the exceedances of their 
maximum allowable joint angles. It is important that the maximum allowable gripping joint 
angles of different gripping models must be determined carefully according to practical 
considerations. 

In addition to gripping an object, the proposed dexterous robot hand may perform several 
hand postures. These hand postures are done via only using the maximum allowable joint 
angles. Fig. 15 shows several typical hand postures for presenting one-digit numbers, 
clenched hand, OK symbol, and YA symbol. 

6. Conclusions 

In this chapter, a dexterous robot hand is proposed based on the design concepts of light 
weight, compact size, similar to human's hand gripping motions, gripping force restriction, 
and low cost. The robot hand prototype is produced in this study. Especially, complicated 
kinematics models and position control of joint angles are not constructed due to 
unpredictable object orientations and positions in this study. Instead, the gripping motion 
behavior models for different gripping motions cooperating with maximum allowable 
tactile forces and joint angles are constructed to simplify the control architecture as well as 
to improve the practical gripping compatibility. This chapter just represent a pilot study of a 
low cost dexterous robot hand; however, most of design and control parameters are not well 
justified to perform perfect performance. On the other hand, the selection of tendon wire as 
well as the mechanical properties and dynamics of the tendon wire are still the major 
interests of the future works. 
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Dimensional Synthesis and Analysis of the 
2-UPS-PU Parallel Manipulator 
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1. Introduction 

Compare with conventional serial robots, parallel manipulators has the advantages of higher 
accuracy, higher stiffness, and higher ratio of force-to-weight, so it has been intensively 
researched and evaluated by industry and intuitions over the last two decades [1]. 
It is well known that a main drawback of parallel manipulator is their reduced workspace. 
Furthermore computing this workspace is not an easy task as, at the opposite of classical 
serial robot, the translational and orientation workspace are coupled [2], A number of 
authors have described the workspace of a parallel mechanism by discretizing the Cartesian 
workspace [3]. In the case of three degree of freedom (3-DOF) manipulators, the workspace 
is limited to a region of the three-dimensional Cartesian space. 

A more challenging problem is designing a parallel manipulator for a given workspace. 
Merlet [4] propounded an algorithm to determine all the possible geometries of Gough-type 
6-DOF parallel manipulators whose workspace must include a desired one. Boudreau and 
Gosselin [5] proposed an algorithm that allows for the determination of some parameters of 
the parallel manipulators using a genetic algorithm method in order to obtain a workspace 
as close as possible to a prescribed one. Kosinska et al. [6] presented a method for the 
determination of the parameters of a Delta-4 manipulator, where the prescribed workspace 
has been given in the form of a set of points. Snyman et al. [7] proposed an algorithm for 
designing the planar 3-RPR manipulator parameters, for a prescribed two-dimensional 
physically reachable output workspace. Laribi et al. [8] presented an optimal dimensional 
synthesis method of the DELTA parallel manipulator for a prescribed workspace. This 
problem was generally solved numerically, and none of the authors mentioned above took 
driving force into account. 

In this paper, the 2-UPS-PU Parallel manipulator is designed to have a specified workspace. 
The algorithm is proposed to solve the optimization problemwhich not only takes into 
account the leg-length limits, the mechanical limits on the passive joints, and interference 
between links, but also the driving forces of the three legs. 



1 Corresponding author. Tel/Fax: +86-335-807-4581. 
E-mail address: yszhao@ysu.edu.cn 
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This paper is organized as follows: Section 2 is devoted to the description of the 2-UPS-PU 
Parallel manipulator. Section 3 deals with the position analysis of the Parallel manipulator. 
Section 4 is devoted to the kinematic analysis of the Parallel manipulator. Section 5 deals 
with the Statics analysis of the Parallel manipulator. In Section 6, we carry out the 
formulation of the optimization problem. Section 7 contains some conclusions. 

2. Displacement analysis 

The 2-UPS-PU parallel manipulator is shown in Fig. 1. This manipulator consists of three 
kinematic chains, including two UPS legs with identical topology and one PU leg, 
connecting the fixed base to a moving platform. In this parallel manipulator, the UPS legs, 
from base to platform, consist of a fixed Universal joint, an actuated prismatic joint and a 
spherical joint attached to the platform. The PU leg connecting the base center to the 
platform consists of a prismatic joint attached to the base, a universal joint attached to the 
platform. This branch is used to constrain the motion of the platform to the three degrees of 
freedom. 



MOVinc 




Fig. 1. The 2-UPS-PU parallel manipulator 



The reference frame O-XbYbZb is fixed on the base and mobile frame L7o-X p Y p Z p is fixed on 
the moving platform (see Fig. 1). At the initial position, the X p -axis and Y p -axis of mobile 
frame are coincidence with the axes of the Universal joints Uo respectively. The orientation 
of the first axis of Lfo is fixed. The orientation of the mobile frame can be represented by 0i 
and 02 shown in Fig. 2, which are two Euler angles about two axes of L7o, respectively. Such 
Euler angles are defined by first rotating the mobile frame about the base x p -axis (first axis 
of (Jo) by an angle 6 X , then about the mobile y p -axis by an angle. For this choice of Euler 
angles, the rotation matrix is defined as 
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R = R X M-Ry(e 1 )- 
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where R x (■) and R r (■) are the basic rotation matrices. 

The position of the mobile frame can be represented by /o, which is the distance between the 
Universal joint Uo and reference point O. The homogeneous transform matrix T, which 
represents the orientation and position of the mobile frame, is 
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cos ft 

sin #, sin ft cos 6 

-cos t sin ft sin 6 





sinft_ 

-sin l cos ft 

cos l cos 2 /„ 

1 



(2) 




Initial orientation 
Fig. 2. The 2-UPS-PU parallel manipulator 



Current orientation 



The spherical joints (Si and S2) of the UPS legs are arranged on the moving platform and 
their distances to the Universal joint Uq on the moving platform is r. The Universal joints (LZi 
and U2) are fixed on the base platform and the distances to the reference point O on the base 
is R. The coordinate of Uo, Si and S2 in mobile frame and the coordinate of Ui and U2 in 
reference frame are expressed as: 
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2.1 Inverse kinematics 

For a given position and orientation of the mobile platform, we can compute the related link 
lengths, denoted by k, using the following relation: 
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where 
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e = rcos/Jsin^sinft + rsin/?cos# 1 -i?sin/? 
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Eq. (4) is the solution of the so-called inverse kinematics problem. 



2.2 Direct kinematics 

If set the second axis of Universal joint Uq pass through either Si or S2 ( a = 0° or a = 90° ), we 
can simply get analytical direct kinematics solution. For example, let a=90°, then cosa=l and 
siiw=0. As a result, Eq. (4) for a = 90° is simplified as: 
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Then, we can calculate d\ and 02 by 
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It is easy to see that Eq. (6) must satisfy: 

0, e [-90° 90°] 
ft e [-90° 90°] 
Obviously, the same calculation can be drawn when a=0°. 
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3. Velocity equation 

To differentiae Eq. (4) allows us to obtain the velocities equation as: 
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where [q] is the kinematic jacobian matrix, and 
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4. Statics analysis 

The workloads can be simplified as a wrench F w applied onto moving platform at L7o. 

F W =(F X F r F_ M x M v M z ) T 

When ignoring the friction in all the joints and the mass of all the parts, the wrench F w is 
balanced by three active forces fi (i = 1,2, 3), two constrained forces f x and f y , and a 
constrained torque m z . Each of fi is applied on and along the axes of three legs; /„ f y and m z 
are applied on the moving platform about X p , Y p , Z p , respectively, the formulae for solving 
active forces and constrained forces are derived as 



F y 
F z 
M 

M 
M 



s t s 2 s, s x s r 

s„, s„ o s 



OP 

f 
/. 
f 

fy 



(8) 



where 
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5.1 Mechanical constraints 

There are four main mechanical constraints that limit the workspace of a parallel 
manipulator [1]: (i) Workspace singularities, (ii) the actuators stroke, (iii) the range of the 
passive joints, and (iv) the link interference. 

5.1.1 Workspace singularities 




^^ 



Fig. 3. The relationship between the determinant of jacobian matrix and 010 2 

The theoretical workspace of the manipulator is surrounded by singular surface. In the 
theoretical workspace, the determinant of jacobian matrix ( | q \ ) should be always greater or 
less than zero. The relationship between the determinant of jacobian matrix and parameters 
of 0i0 2 , for r =150, R =200, «=n/2, /?=n/6 and l =300, is shown in Fig. 3. If we set 0i=0 2 =O as 
the initial oriention, the determinant of jacobian matrix should be always greater than zero. 



\q\>0 



(9) 



5.1.2 Actuators' stroke 

The limited stroke of actuator i imposes a length constraint on link i, such that 



^m ^-<0.8 i = 0,1,2 



(10) 
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where /j m i n and /j max are, respectively, the minimum and maximum lengths of leg i. 

5.1 .3 Range of the passive joints 

Each passive joint has a limited range of motion. Let the maximum misalignment angle of 
the Universal joint 17, be diumax, Then, the limits on Universal joint 17, impose a constraint, 
such that 



Similarly, Let the maximum misalignment angle of the spherical joint S, be 0, s 
limits on spherical joint Si impose a constraint, such that 



(11) 
Then, the 



*«***.« (12) 

5.1.4 Link interference 

Let us assume that the links can be approximated by cylinders of diameter D. This imposes a 
constraint on the relative position of all pairs of links, such that 

distance^ l)>D i, j = 0, 1, 2 i* jl (13) 

or the minimum distance between every two line segments corresponding to the links of the 
parallel manipulator should be greater than or equal to D. The minimum distance between 
two line segments is not given by a simple formula but can be obtained through the 
application of a multi-step algorithm. Due to space limitations, we will not present that 
algorithm here but refer the reader to the well-detailed one given in [9]. 

5.2 Workspace representation 




i 


K 






j£> 





According to the characteristics of the 2-UPS-PU parallel manipulator, we present a 
cylindrical coordinate system (see Fig. 4), where (p and y are exactly the polar coordinates 
representing the orientation of the moving platform and lg is the z-coordinate. 
The unit vector of Zp-axis in reference frame is expressed as: 



^sin/cos")^ I 



smysmcp 
cos/ 



sin# 2 
-sin t cos 6 2 
cos 6. cos 6. 



(14) 
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Then, we can calculate d\ and Qi by 



2 = sin '(sin/cos^a) 
sin y sirup 



0, 



cos ft 



(15) 



5.3 Algorithm for the workspace 

Step 1: Initialize double arrays A, B and Z, with dimensions (n+2)xm, where n is the 
number of equally spaced planes /o between hmm and Zomax at which the 
workspace will be computed, and m is the number of points to be computed at 
each plane /o=const. These arrays will store, respectively, the values of (p , y and 
/o for the points defining the workspace boundary. 

Step 2: Set r ,R,h ,a , /? and D. 

Step 3: Set # max , 6>. max , / and / , Where i = , 1 , 2 . 

Step 4: Set /o=/omin. 

Step 5: for the current lo, construct a polar coordinate system at (q> , y\ . Starting at m 
equally spaced angles, increment the polar ray, solve the inverse kinematics, 
and apply the constraint checks defined by Eqs. (8) ~ (12) until a constraint is 
violated. The values for <p , y and !o at the point of constraint violation are 
written into the three double arrays. 

Step 6: Set l =l + A/ , where A/ = -^^ — °-^- 

n 

Step 7: Repeat steps 5-6 until /o becomes greater than /omax- 

Step 8: Transfer A and B into X and V, so that X(i,j) = B(i,j)-cos[A(i,j)~] and 

Y(i,j)=B(i,jYsm\ A(i,j)], where ; = l---n + land j = l---m. 
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Fig. 5. Isometric view of the Parallel manipulator's workspace. 
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The proposed algorithm was implemented in MATLAB for the 2-UPS-PU parallel 
manipulator whose data are r = 100 , R = 120 , a = 90° , p = 25° , D = 5, l mm = 320 , / max = 578 , 
'omin =320, l 0ma =570, Umax =90°, and Sma =60° . The workspace of the manipulator is 
presented in Fig. 5. and the approximated projected workspaces for /o=350, 450 and 500 
shown in Fig. 6. 



180 




Fig. 6. The projected workspace of the Parallel manipulator for the positions (a) 10=350, (b) 
10=450 and (c) 10=500 

6. Optimal design 

The aim of this section is to develop and to solve the multidimensional optimization 
problem of selecting the geometric design variables for the 2-UPS-PU parallel manipulator 
having a prescribed workspace with better driving capability. 

The prescribed workspace of the parallel manipulator is defined as a cylinder with radius y 
and height h (see Fig. 7) and the actual workspace is convex (see Fig.5). The prescribed 
workspace is inside the workspace of the parallel manipulator if all the points in the 
boundary curves are inside the actual workspace. 




Fig. 7. The scheme of the prescribed workspace. 
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6.1 Objective function 

The parameters to be optimized are the minimum lengths of three legs (lomm, hmm and /2mm), 
the dimension of the base and the platform [R , r) , and the relative angular position of the 

two UPS chains (a , /?) . Without losing generality, let r be normalized by h, and R, lomm be 
normalized by r such that 

K = r - K=- K=— (16) 

n r r 

The objective function of the multi-parameter optimization problem, can be stated as 

min k = 7rR 2 L F 

On,," (17) 

Subject to / (/, p) < for all the points p inside the specified workspace, 
where I is the unknown vector of parameters, and F is the maximum of the driving force of 
the three legs. 

6.2 Algorithm for dimensional synthesis 

Step 1: Initialize double arrays A, B, C and D, with dimensions 361x1. Set the 
parameters y u and h representing the prescribed workspace. 

Step 2: Set a = 90° , k = , and the allowable parameter ranges for ji ,k r , k R and k, 

Step 3: Set the cycle number n. 

Step 4: Random select the parameters ji , k r , k R and k, by Monte Carlo method in 

ranges. 
Step 5: Set l = k- l 0mm and y = y u , Starting at 360 equally spaced angles, increment the 
angle cp , solve the inverse kinematics, write the length of the UPS legs into 
array A and B. set /. =/, = min(min(,4),min(2?)), and /,„„=/,„,. =1.8 ■/.„„ . 

j 1 mm 1 mm ^ \ / \ / J ' max 1 nidx 1 min 

Step 6: Set flag=0. For the current h, construct a polar coordinate system at [cp , y) . 

Starting at 360 equally spaced angles, increment the polar ray, solve the inverse 
kinematics, and apply the constraint checks defined by Eqs. (8)-(12) until y > y u . 
Set flag=l at the point of constraint violation and return to Step 12. 
Step 7: If flag=0, then set l =k ■ l 0ab + h . For the current /o, construct a polar coordinate 

system at [cp , y) . Starting at 360 equally spaced angles, increment the polar ray, 

solve the inverse kinematics, and apply the constraint checks defined by Eqs. 

(8)-(12) until y > y„ ■ Set flag=l at the point of constraint violation and return to 

Step 12. 
Step 8: If flag=0, then set l =k / 0min and y = y u . Starting at 360 equally spaced angles, 

increment the angle q> , solve the Statics, and write the maximum driving forces 

of the three legs into array C. 
Step 9: If flag=0, then set l a =k-l 0lma +h and y = y„ ■ Starting at 360 equally spaced 

angles, increment the angle tp, solve the Statics, and write the maximum driving 
forces of the three legs into array D. 



Dimensional Synthesis and Analysis of the 2-UPS-PU Parallel Manipulator 



277 



Step 10: Set F _ max (max(C) , max(D)) , and k d = FRl 0mm . 

Step 11: If k = or k d < k , set O, = k, ,0 R =k R , 0, a = k, a , O^ = / lml0 and 0,_ = / lmax . 

Step 12: Repeat steps 4-11 until the cycle time is n. 
The proposed algorithm was again implemented in MATLAB. The optimal solution 
obtained for this example is presented in Table 1 for y u = 60° and h = 100 . 
Fig. 8 shows the workspace of the 2-UPS-PU parallel manipulator. 

Fig. 9 shows the relationship between the minimum of y and la. One can notice the actual 
workspace include the prescribed workspace. 



p 


R 


r 


'l,2min 


'l,2max 


'Omin 


'Omax 


IT 


186 


124 


396 


712 


450 


650 



Table 1. The optimal dimensions of 2-UPS-PU parallel manipulator 




Fig. 8. Isometric view of the Parallel manipulator's workspace. 
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Fig. 9. The relationship between the minimum of y and 10 
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7. Conclusion 

This paper proposed a novel 2-UPS-PU parallel manipulator with simple structure, high 

rotational capability, high load carrying capacity. 

Based on the cylindrical coordinate system, an algorithm for computing three-dimensional 

workspace of the manipulator has been proposed in this paper. The boundary of the 

workspace on the specific plan is found out quickly by step-searching along the selected ray 

line. 

An optimal dimensional synthesis method was presented to optimize this manipulator for a 

prescribed workspace. The driving force parameters were introduced into the object 

function. All the dimensional parameters, including the length of the legs, were included 

into the optimizing algorithm. 

The methods proposed in this paper can be adopted universally. The results of this paper 

provide solid theoretical basis for further theoretical studies and practical application of this 

manipulator. 
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1. Introduction 

During the last two decades, there has been significant progress in the area of adaptive 
control design of nonlinear systems (Krstic et al., 1995; Sastry & Isidori, 1989; Slotine & Li 
1991; Spooner et al., 2002). Most of the developed adaptive control schemes assume that an 
accurate model of the system is available and the unknown parameters appear linearly with 
respect to known nonlinear functions. However, this assumption is not sufficient for many 
practical situations, because it is difficult to precisely describe a nonlinear system by known 
nonlinear functions and, therefore, the problem of controlling nonlinear systems with 
incomplete model knowledge remains a challenging task. 

As a model free design method, fuzzy control has found extensive applications for complex 
and ill-defined plants (Passino & Yurkovich, 1998; Wang, 1994). Basically, fuzzy control is a 
human knowledge-based design methodology which is driven accordingly by fuzzy 
membership functions and fuzzy rules. However, it is sometimes difficult to find the 
matched membership functions and fuzzy rules for some plants, or the need may arise to 
tune the controller parameters if the plant dynamics change. In the hope to overcome this 
problem, based on the universal approximation theorem and on-line learning ability of 
fuzzy systems, several stable adaptive fuzzy control schemes have been developed to 
incorporate the expert knowledge systematically (Spooner & Passino, 1996; Spooner et al., 
2002; Su & Stepanenko, 1994; Wang, 1994). The stability analysis in such schemes is 
performed by using the Lyapunov approach. Conceptually, there are two distinct 
approaches that have been formulated in the design of a fuzzy adaptive control system: 
direct and indirect schemes. The direct scheme uses fuzzy systems to approximate unknown 
ideal controllers (Chang, 2000; Chang, 2001; Labiod & Boucherit, 2003; Li & Tong, 2003; 
Ordonez & Passino, 1999; Spooner & Passino 1996; Wang, 1994), while the indirect scheme 
uses fuzzy systems to estimate the plant dynamics and then synthesizes a control law based 
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on these estimates (Boulkroune et al., 2008a; Boulkroune et al., 2008b; Chang, 2000; Chang, 
2001; Chekireb et al., 2003; Chiu, 2005; Golea et al, 2003; Labiod et al., 2005; Ordonez & 
Passino, 1999; Spooner & Passino 1996; Su & Stepanenko, 1994; Wang, 1994). 
For uncertain single-input single-output (SISO) nonlinear systems, fuzzy adaptive control 
schemes were proposed in (Chang, 2001; Essounbouli & Hamzaoui, 2006; Labiod & 
Boucherit, 2003; Spooner & Passino, 1996; Su & Stepanenko, 1994; Wang, 1994). The problem 
of adaptive fuzzy control of uncertain multi-input multi-output (MIMO) nonlinear systems 
is more difficult because of the coupling that exists between the control inputs and the 
outputs. This problem was studied in (Boulkroune et al., 2008a; Boulkroune et al., 2008b; 
Chang, 2000; Chekireb et al., 2003; Chiu, 2005; Golea et al., 2003; Labiod et al., 2005; Li & 
Tong, 2003; Ordonez & Passino, 1999; Tlemcani et al, 2007 ; Tong et al., 2000; Zhang & YI, 
2007). We note that the direct adaptive approach turns out to require more restrictive 
assumptions than the indirect case, but is perhaps of more interest because it does not 
present any possible controller singularity problem. 

In the aforementioned papers, the adjustable parameters of the fuzzy systems are updated 
by an adaptive law based on a Lyapunov approach, i.e., the parameter adaptive laws are 
designed in such a way to ensure the convergence of a Lyapunov function. However, for an 
effective adaptation, it is more judicious to directly base the parameter adaptation process 
on the identification error between the unknown function and its adaptive fuzzy 
approximation (Labiod & Guerra, 2007a; Labiod & Guerra, 2007b). 

This chapter presents direct and indirect adaptive fuzzy control schemes for a class of 
continuous-time uncertain MIMO nonlinear dynamic systems. The proposed schemes are 
based on the results in (Labiod & Guerra, 2007a). In the direct approach, since fuzzy systems 
are used to approximate unknown ideal controllers, the adjustable parameters of the used 
fuzzy systems are updated using a gradient descent algorithm that is designed to minimize 
the error between the unknown ideal controllers and fuzzy controllers. On the other hand, 
in the indirect approach, since fuzzy systems are used to approximate the system's 
unknown nonlinearities, the adjustable parameters of the used fuzzy systems are updated 
using a gradient descent algorithm that is designed to minimize the error between the 
system's unknown nonlinearities and the used fuzzy systems. In both approaches, the 
stability analysis of the closed-loop system is performed using a Lyapunov approach. In 
particular, it is shown that the tracking errors are uniformly ultimately bounded and 
converge to a neighbourhood of the origin. 

The organization of this chapter is as follows. The problem formulation and fuzzy systems 
description are given in section 2. The MIMO direct adaptive fuzzy controller with a proof 
of the stability results are presented in section 3. The MIMO indirect adaptive fuzzy 
controller with its stability analysis is given in section 4. Section 5 presents simulation 
results of the proposed direct adaptive control scheme applied to a two-link robot 
manipulator. Finally, section 6 concludes the chapter. 

2. Problem formulation 

We consider a class of uncertain MIMO nonlinear systems modeled by 
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i/i"* =/:(*) + !>,(*)' 



(1) 



M . 



: /»+i:=a(*)> 



where x - 



y t , y u ... ,y ( [ 1 ' 1) ,...,y p , y p , 



>y, 



;-*) 



e R" with n = } r, , is the overall state 



vector which is assumed available for measurement, u= \u 1 ,...,u\ el' is the control 

input vector, y = \y 1 ,---,y„ \ s R p is the output vector, and fAx), Sa( x ) ' i>j = ^>--->P are 

unknown smooth nonlinear functions. 
Let us denote 



,W. 



^ < 



/(*) = [/t(*),...,/,(*)] T ;G(*) = 



guW ■■■ 8i P ( x ) 
g P i(x) ■■■ gpp( X ) 



Then, dynamic system (1) can be written in the following compact form 

y^=f{x) + G(x)u 



(2) 



The control objective is to design adaptive control u { (f) for system (1) such that the output 
y t (t) follows a specified desired trajectory y i{ (t) under boundedness of all signals. 
Throughout this study we need the following assumptions. 
Al: The matrix G(x) is symmetric positive definite and bounded as < gl <G(x)<gI , 

where I is the p x p identity matrix, g and g are some positive constants. 

A2: The desired trajectory i/,,,(f) is a known bounded function of time with bounded 
known derivatives up to the r l order. 

Remark 1: Notice that Assumption Al is a sufficient condition ensuring that the matrix 
G(x) is always regular and, therefore, system (1) is feedback linearizable by a static state 

feedback. Although this assumption restricts the considered class of MIMO nonlinear 

systems, many physical systems, such as robotic systems (Slotine & Li, 1991), fulfill such a 

property. 

Define the tracking errors as 

i (3) 

*,(t)=y*(*)-y„(*) 



and the filtered tracking errors as 
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»,c)=(|hJ"\(')^>° 



(4) 



From (4), s ; (f) = represents a linear differential equation whose solution implies that the 
tracking error e,(f) and its derivatives up to r i -1 converge to zero (Slotine & Li, 1991). 
Thus, the control objective becomes the design of a controller to keep s ; (f) at zero, 
i = 1, . . . , p . Moreover, bounds on s { (t ) can be directly translated into bounds on the tracking 
error. Specifically, if s,(f) <0 ; where ; is a positive constant, one can conclude that 
(Slotine & Li, 1991): lej'^f)] < 2'Af ,;+1 <D,, ; =0,...,r, -1 , i = l,...,p. These bounds can be 

reduced by increasing the parameters X i . 

The time derivatives of the filtered errors (4) can be written as 



K=v r -f r {x)-Y^gA x ) u i 

where v 1 ,...,v , are given as follows 

p 1 =yS ) +A„ l -iel r,_1) +...+A,A 

with 



(5) 



(6) 



Denote 



P i4 =C^\^;' ] > t = l/-P/ j=l,...,r i -l 

s = [s 1 ,...,s p ] ; v = [v u ...,v p ~\ 

Then equation (5) can be written in matrix form as 

s = v-f(x)-G(x)u (7) 
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If the nonlinear functions f(x) and G(x) are known, to achieve the control objectives, one 
can use the following ideal nonlinear control law (Labiod & Guerra, 2007b) 

u =G- 1 (x)(-f(x) + v + Ks + K o tanh(s/£ )) (8) 

where, K = diag\k 1 ,...,k\ , K = diag\k n ,...,k \, with A: ; >0 and fe o; >0,for i = l,...,p, 
s a is a small positive constant, and tanh() is the hyperbolic tangent function defined for 

the vector s= s 1 ,...,s as tanh(s/f ) = tanh(sj/f ),...,tanh(s /f ) . 
Effectively, when we select the control input as u = u , equation (7) simplifies to 

s = -Ks-K t<mh(s/e ) (9) 



or, equivalently 



s,- = -k i s i :-k oi tanh(s,. /£■„), i = l,...,p (10) 



From which we can conclude that s i (t)—>0 as t — > co and, therefore, e,(f) and all its 

derivatives up to j- - 1 converge to zero. 

It is clear that if f{x) and G{x\ are completely unknown, the proposed nonlinear control 

law (8) is not feasible. In this case, in order to overcome this design difficulty, we propose to 
use fuzzy systems to construct adaptively the unknown functions. The idea is to use fuzzy 
systems to identity the entire unknown control function (8) in the direct approach, and to 
identify the unknown nonlinear functions fix) and G(x) in (8) in the indirect approach. 
In this chapter we use the zero-order Takagi-Sugeno fuzzy system that performs a mapping 
from an input vector z = \z 1 ,...,z n ,1 e Cl 2 cl* to a scalar output variable y.eK., where 

Q_ = f2, X'»xQ 1 and Q z cz R . If we define M,- fuzzy sets F t , ] = l,...,M j , for each input 
Z t , then the fuzzy system will be characterized by a set of if-then rules of the form (Wang, 
1994; Jang & Sun, 1995; Passino & Yurkovich, 1998) 

R k :Ifz 1 isG 1 i: and...andz„,isGj,Then y f isy k f (k=l,...,N) 

where G* e lF^,...,F j M ' \ , i = l,...,n, y k f is the crisp output of the k -th rule, and N is the 

total number of rules. 

By using the singleton fuzzifier and the product inference engine, the final output of the 

fuzzy system is given as follows (Wang, 1994; Jang & Sun, 1995; Passino & Yurkovich, 1998) 
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y/W' ^ff (ii) 

where 

^ ( z ) = n"i^G} ( z .-)' with ^ G ? e {^- '-'^v 

where fj , {x { ) is the membership function of the fuzzy set F/ . 

By introducing the concept of fuzzy basis functions (Wang, 1994), the output given by (11) 
can be rewritten in the following compact form 

y f {z) = w r {z)8 (12) 

where 6* =y *,..., y^M is a vector grouping all consequent parameters, and 
w{z) = \w 1 {z),...,w N (z)] is a set of fuzzy basis functions defined as 

w t {z) = -^- k = l, ...,N (13) 

The fuzzy system (12) is assumed to be well-defined so that V . fi, ( z ) ^ f° r an zsCl z . 

It has been proved in (Wang, 1994) that fuzzy systems in the form of (12) with Gaussian 
membership functions can approximate continuous functions over a compact set to an 
arbitrary degree of accuracy provided that enough number of rules are considered. So, for a 
general smooth nonlinear function f(z) defined from R" to R , there exists a fuzzy system 

in the form of (12) with some optimal parameters 9" such that 

where e is a positive constant. Thus, one can express f(z) as 

f(z) = w T (z)0" +s{z), 

where s(z) is the fuzzy approximation error satisfying U(z) ^ e for z s CI, . 

In this chapter, it is assumed that the structure of the fuzzy system and the fuzzy basis 
function parameters are properly specified in advance by the designer. This means that the 
designer decision is needed to determine the structure of the fuzzy system (that is, 
determine relevant inputs, number of membership functions for each input, membership 
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function parameters, number of rules), and the consequent parameters should be calculated 
by learning algorithms. 

It should be noticed that fuzzy systems can be replaced by any other linearly parameterized 
universal function approximator without any technical difficulty such as neural networks 
and wavelet networks. However, only fuzzy logic systems can make use of linguistic 
information in a systematic way. 

3. Direct adaptive fuzzy control 

In section 2 we have established that there exists an ideal control law u given by (8) that 
can achieve the control objective. However, this nonlinear controller cannot be used since it 
depends on unknown functions. In this section, to circumvent this problem, we propose to 
use adaptive fuzzy systems for approximating this ideal controller, and the error between 
the fuzzy controller and the ideal controller will be used to update the free parameters of the 
fuzzy controller. 
To develop the control law, we represent each component of the ideal input control vector 

u =\u 1 ,...,u\ by a fuzzy system in the form of (12) as the following 

u'(z) = wj(z)0* +£ { (z); i = l,...,p (14) 

where 2 = ^' ,s' 1 , sAz) is the fuzzy approximation error, 0" is an unknown ideal 
parameter vector that minimizes the function \g. (z) over an operating compact set Q. z , and 
w i (z) is a fuzzy basis function vector assumed suitably specified by the designer. In this 

study, we assume that the used fuzzy systems do not violate the universal approximation 
property on the compact set Q, z , which is assumed large enough so that the variable z 
remains inside it under closed-loop control. So it is reasonable to assume that the fuzzy 
approximation error is bounded for all z e Q 2 . 
Let us denote 

e(z) = [e t (z),...,e r {z)J ; ? =[ti?,...,0?J , w{z) = diag[w 1 {z),...,w p {z)] 

Therefore, one can write (8) as 

u =w'\z)0" +e(z) (15) 



Since the ideal parameter vector is unknown, let us use its estimate instead to form 
the adaptive control 



u(z) = w T (z)0 (16) 
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The next step should be the design of an adaptive law for the free parameters such that 
the control law u approximates, as best as possible, the ideal controller u . To this end, let 
us define the error between the controllers u" and u as: 

e u =u-u (17) 

The error e l( represents the actual deviation between the unknown function u and the 
online fuzzy approximator (16), while the fuzzy approximation error s (z) represents the 

minimum possible deviation between the unknown function u and the online fuzzy 
approximator, i.e. e(z) represents the minimum possible value of e u . 
Using (15) and (16), (17) becomes 

e„ = u" - w 1 (z)0 = w T (z)0 + e(z) (18) 

where = - 6 is the parameter estimation error vector. 

Adding and subtracting G(x)« to the right-hand side of (7), we obtain the error equation 

governing the closed-loop system 

s = v-f(x)-G(x)u + G(x)u"-G(x)u" (19) 

With (8) and (18), (19) becomes 

s = -Ks-K t<mh(s/s ) + G(x)e u (20) 



Now, consider a quadratic cost function; that measures the discrepancy between the ideal 
controller and the actual fuzzy controller, defined as 



](0) = ^lG(x)e, 1 =-(u'-w T (z)6) r G(x)(u-w T (z)6) (21) 



We use the gradient descent method to minimize the cost function (21) with respect to the 
adjustable parameters 6 . Consequently, applying the gradient method (Slotine & Li, 1991; 
Ioannou & Sun, 1996), the minimizing trajectory 8(t) is generated by the following 
differential equation 

= - !? V o J{0) (22) 
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where r/ is a positive constant parameter. 

From (21), the gradient of ](0) with respect to is 

d M = - w{z )G(x)e u 

80 ' y ' 

Therefore, the gradient descent algorithm becomes 

= r/w(z)G(x)e u (23) 

We recall here that the ideal controller u is unknown, so the error signal e u defined in (17) 
is not available. Equation (20) will be used to overcome this design difficulty. Indeed, from 
(20), we see that even if the error vector e n is not available, the vector G(x)e u is available, 
and it is given by 



Therefore, (23) becomes 



G(x)e n = s + K s + K tanh(s/ 1 



d = r}w(z){s + Ks + K t<mh(s/£ j) (24) 



As shown by (Ioannou & Sun, 1996), an adaptive law in the form of (24) cannot guarantee 
the boundedness of the parameters in the presence of approximation errors, which are 
unavoidable in such adaptive schemes. So, to improve the robustness of the adaptive law 
(24) in the presence of approximation errors, we modify it by introducing a a -modification 
term as follows (Ioannou & Sun, 1996) 

= tjw(z){s + Ks + K tanh(s/f )} - 77 cr0 (25) 



where a is a small positive constant. 

The following theorem summarizes the stability result for the proposed direct adaptive 

control scheme. 

Theorem 1: Consider the system in (1) with the control law defined by (17). Suppose that 

Assumptions Al and A2 hold, the approximation error e(z) in (18) is bounded as \\s{z) <£ where 

e is a positive constant, and that the free parameters are updated according to (25). Then, all the 
closed-loop signals are uniformly ultimately bounded, and the tracking errors are attracted to a 
neighbourhood of the origin whose size can be adjusted by control parameters. 

Proof: Let us consider the following Lyapunov function candidate 
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V = -s's + —0 T (26) 

Differentiating (26) with respect to time and using (20) and (25), we get 

V = -s'Ks-s T K o tanh(s/£ o ) + s T G(x)e ll -0 T (w(z)G(x)e ll -cT0) (27) 

With (18), (27) becomes 

V = -s T Ks- s T K tanh(s/^ ) + s r G(x)e u - ejG(x)e u + s r (z)G(x)e u + a0 T (28) 

Using the following inequalities 

aFe<-^\\ef+-¥t 

2 II II 2 11 " 
£ '(z)G(x)e, l <^ejG(x)e„ +E '(z)G(x) £ (z) 



^G{x)e, l <^ejG{x)e, l +h T G{x) S 



we have 



V<-s r \ K--G(x))s-s'K o tetnh(s/£ o )--ejG(x)e u --J0f + e(z) T G(x)£(z) + —\\0'f (29) 



Since s(z) and G(x) are assumed bounded in this study and is a constant vector, we 
can define a positive constant bound i// 1 as 

Wi =sup t (^(z) , G(*)£(z)J + -o-|0*|| 
Then, (29) can be simplified to 

V <- S T U-^gl)s-^\\0f - S T K tanh{ S /s )-^ejG(x)e u+Wl (30) 



We assume here that each design parameter k t is chosen such that k j > g/2 and we let 
k = 2min 1SlS [k i - g/2) . Consequently, (30) can be bounded by 
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\ 
V<-a 1 V-s , K tanh(s/£ ) ejG{x)e u +y/ l <-a 1 V + y/ 1 (31) 

where a l =min(x', crrj) . 

From (31), one can establish that the Lyapunov function candidate satisfies the following 
condition 

(32) 



0<V(f)<(y(0)-^-|e-' + ^- 

l « J «1 



This last condition implies that s(f) and #(f) are uniformly bounded, and that s(f) is 
uniformly ultimately bounded with respect to the set fi s =<s:||s||< J2y/ 1 /a 1 \ . This 
consequently leads to uniform boundedness of the tracking errors 
\ef {t)\<2' Xf' +1 ^ly/J ai , j=0,...,r i -l, i = l,...,p (Slotine & Li, 1991). 

Remark 2: In the absence of the approximation error, i.e., e(x) = in (18), by setting a = 
in (25), one can show that the tracking errors are asymptotically stable, i.e., eJt) — >0 as 
t — >oo, for i = l,...,p . 

Remark 3: It is worth noticing that the parameter updating law (25) is not implementable in 
case the derivative of s(f) is not available. However, a discrete implementable version of 
(25) can be obtained. Rewriting (25) as 

t t 

0(t) = 0(t-M)+ J ^(r)s(r)dr+ J 2 (r)dr , 

where Af is a small positive constant, (j> x =r]w{z) and 

2 =tjw(z)(Ks + K tanh ( s/£- ))- 77 cr# . Using the fact that s = ds/dt , the expression of 0(t) 
becomes 

0(t) = 0(t-At)+ J 1 (r)ds+ J 2 (r)dT. 

s(t-M) t-At 

By assuming that ^(f), ^ 2 (0 anc ^ s (0 are continuous time functions and that Af is small 
enough, a discrete implementable version of (25) is given by: 
0(t) = 0(t-M) + <fa(t-M)(s(t)-$(t-At)) + 1 (t-At)M, which represents a good discrete 
approximation of the parameter update law (25) if Af is chosen sufficiently small. 

4. Indirect adaptive fuzzy control 

In this section we propose to indirectly approximate the unknown ideal controller (8) by 
identifying the unknown functions f { (x) and g«(x) using fuzzy systems. First, let us 
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assume that the nonlinear functions fAx) and g,,(at) can be approximated, over a compact 
set D x , by fuzzy systems of the form of (12) as follows 



f l (x) = f-(x) + £f (x); f-(x) = w T f (x)e; , i=l,...,p 

?#(*) = gj(*)+ *«,(*); *#(*)= «£(*)** i 'i= 1 '--'V 



(33) 
(34) 



where e, (at) and e (at) are fuzzy approximation errors, 6** and 0" are optimal 
parameter vectors that minimize functions £7 (at) and £ (at), respectively, and w f (at) 
and a; (at) are fuzzy basis function vectors assumed suitably specified by the designer. 

In this study, we assume that the used fuzzy systems do not violate the universal 
approximation property on the operating compact set D x , which is assumed large enough 
so that state variables remain within D x under closed-loop control. So it is reasonable to 
assume that the minimum approximation errors are bounded for all at e D x . 
Since the ideal parameter vectors 0, and 9" is unknown, let us use their estimates 0, and 
instead to form the adaptive approximations 



f i {x) = w T f(x)0 f , i = l,...,p 



(35) 
(36) 



Denote 



/(*) = [/,(*),. ..,/„(*)] r , e f (x) = [e n (x),..., £) ,(x)] T 



G(X)- 



, e G {x)-- 



"sii 



°gpi 



(*) 



(*) ... v(-) 



gn(x) ■■■ gi,,( x ) 

Jpi( x ) -■ Spp( x ) 
Now we can write an expression for the adaptive control law 

u = G- 1 (x)(-f(x) + v + Ks + K tanh(s/e )\ 



*(*)' 



(37) 



This control term results from (8) by using the adaptive fuzzy approximations /(at) and 

G(x) instead of actual functions /(at) and G ( at ), respectively. 

Adding and subtracting /(at) and G(at)u to the right-hand side of (7), we get 
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6 = v-(f(x)-f{x))-(G{x)-G{x))u-G{x)u-f(x) (38) 

Using the control law (37), (38) becomes 

s = -Ks-K o tanh(s/£ )-(f(x)-f(x))-(G(x)-G(x))u (39) 

where each element of the vector s is given by 

s, = -k t s, - k m tanh(s ( /s ) - (/, (*) - /,(*)) -£(g, (*)- 1, 5 (*))«, (40) 

The next task should be the design of adaptive laws for the free parameters fi and t . such 
that f(x) + G(x)u approximates, as best as possible, the unknown nonlinear function 
/(x) + G(x)m . To this end, let us define the modelling error e m between /(x) + G(x)m and 
/(*) + G(x)w as: 

e m =(f(x) + G{x)u)-(f(x) + G{x)u) = (f(x)-f(x)) + (G{x)-G(x))u (41) 

where each component of the vector e m is given by 

^=(/«(*)-/«(*))-tu#(*)-# ? (*))^=(/;(*)-/ < (*))+iu;(*)-# ? (*))«,+*,(*) («) 

e- = K (*K, + £< ( x )i, u < + £ > (*) ( 43 ) 

v 
with £ ; (x) = £ fi (*) + X £ x, (*)"; ' an d & = #/, "^ and &.. =@" s , ~# Sr , ■ 

Then, from (39) and (41) we have 

e :l ,=-{s + Ks + K tanh(s/ £o )) (44) 

Now, consider a quadratic cost function; that measures the discrepancy between the 
unknown nonlinearities and their adaptive fuzzy approximations, defined as 



/w=ke»=fi:f(/«(*)-/«(*))+i:u 9 (*)-* f (*))«, 



2tt 



(45) 



Applying the gradient method, the minimizing trajectories dAi) and &~ (f) are generated 
by the following differential equations 
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K,=-' y .,/(«) 

where 7 is a positive constant parameter. 
Therefore, the gradient descent algorithm becomes 

fl =r,w fi (x)e mi 



Since the modelling error e m is not available, equation (44) will be used to overcome this 
design difficulty. Then, we obtain 

\ff fi =-ijw fi (x)(s + k i s i +k oi tarih(s i /£ )) 
[0 gij = -rjw gij (x)u t (s + fc,s, + k m tanh(s,/£ )) 



In order to improve the robustness of the adaptive law (48) in the presence of approximation 
errors, we modify it by introducing a a -modification term as follows (Ioannou & Sun, 1996) 

[e fi =-j 7 w fi (x)(s + k i s i + k Oi tanh(s l /s o ))-7 ] (T0 fi 

[0 gij = -?jw glj (x) Uj (s + fc,s, + k 0i tanh(s ; /f )) - rjcrff^ 



where <j is a small positive constant. 

Before proceeding we need to introduce an assumption about the approximation errors 

£(x) = s f (x) + £ G (x)u . Since s(x) depends upon the control input u, s(x) is not assumed 

bounded by constant bounds but it is assumed bounded by functional bounds. 

A3: The function e (x) = s , (x) + s G (x)u is bounded as follows 



e(x)\\< S +Y,^sf ; S i >0,i = 0,...,p. 



The following theorem summarizes the stability result for the proposed indirect adaptive 
control scheme. 

Theorem 2: Consider the system in (1) with the control law defined by (37). Suppose that 
Assumptions A1-A3 hold and that the free parameters of the used fuzzy systems are updated 
according to (49). Then, all the closed-loop signals are uniformly ultimately bounded, and the 
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tracking errors are attracted to a neighbourhood of the origin whose size can be adjusted by control 
parameters. 

Proof: Let us consider the following Lyapunov function candidate 



1 ' I 1 1 f 

v=-y sf+-0' f f +-y0j0 s 



(50) 



Differentiating (26) with respect to time and using (40), (43), (44) and (49), we get 



V = J -krf - *oA tanh( S; / fo ) - s, e ,„, - e „„ ( e „„ - e, (*)) + ff^fo + tJ^ 



;=i 



(51) 



Using the following inequalities 



-t (Til- II 2 (Til .II 2 -t (Til- II 2 

o0]0, <--\e,\ +-\\0 f \\ , <j0 t ,0, <-—\e, - 

£ , OK; < -^l« + ef 0) > s,e, m < -e 2 mi + s 1 



we obtain 



V = 2 -{ k , - !) s ? - Kfi, tanh 



sj 1 2 o'lirt II 2 a \\n-\\ 2 v-srWn II 2 < T v , ll/i" II 2 It \ 
— \--eii — \\0 e \\ +—\\0f\\ — > \\0. + — / \\0. +st\x) 

*o 7 z z z z i=i z /=i 



(52) 



2 . 

Since W(#) =y^£ ( 2 (x) and using assumption A3, we have 



*=X 



( A; - S - l)sf - k ol S t tanh | ^- - - <£, 1 4- .//, 



(53) 



with 



i=l I z z j-1 



We assume here that each design parameter k l is chosen such that k t >{8 i — 1) and we let 
«r = 2min 1SlSp (fc ; - S t - 1) . Consequently, (53) can be bounded by 
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V <-a 2 V - s r K tanh(s/.? ) ej e m + y/ 2 <-a 2 V + y/ 2 



(54) 



where a 1 =min(K, arj) . 

From (54), one can establish that the Lyapunov function candidate satisfies the following 
condition 



o<v(f)<(y(o)-^| e -" j '+^ 

I «2 J «2 



(55) 



This last condition implies that s(i), 6 f (f) and 9 [i) are uniformly bounded, and that 
s(t ) is uniformly ultimately bounded with respect to the set fi s = I s : llsll < J2y/ 2 /a 2 > . This 
consequently leads to uniform boundedness of the tracking errors 
\e\> ] (t)\<2> Af'- +l ^2y 2 l a 2 , ;' = 0,...,r, -1 , i = l,...,p (Slotine & Li, 1991). 

Remark 4: Since the matrix G(x) is generated on line, the control law (37) is not well- 
defined if G(x) becomes not regular. To overcome this singularity problem, we use a 

regularized inverse as in (Labiod et al., 2005) given by G" 1 (x) a G T (x)\ y l +G[x)G l (x) , 
where y is a small positive constant. 

Remark 5: In the absence of the approximation error, i.e., s (x) = , by setting a = in (49), 
one can show that the tracking errors are asymptotically stable, i.e., e- ( f ) — > as t — > oo , for 
i = l,...,p . 



ill' 
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5. Simulation results 

In this section, we test the proposed direct adaptive fuzzy control scheme on the tracking 
control of a two-link rigid robot manipulator with the following dynamics (Labiod et al., 
2005; Slotine & Li, 1991; Tong et al., 2000): 

7v — hh —hin -4- h \ n I 

(56) 

iy 2 J \jvi 21 -iVI 22 J II " 2 J n Hl u L^2jl 

where 

M u = a x +2fl 3 cos(£j 2 ) + 20 4 sin(<7 2 ) , M 22 = a 2 ,M 21 =M 12 = a 2 + a 3 cos(q 2 ) + a 4 sin(q 2 ) , 

h = a 3 sin(q 2 ) - a i cos(q 2 ) , 
with 

a 1 =I 1 + tnj-c! + I e + m e l^ e + m e \\ , a 2 = I e + m e l 2 ce , a 3 = m e l 1 l ce cos S t , a 4 = tn e IJ CI , sin S e . 
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In the simulation, the following parameter values are used 



m x = 1, m r = 2, Z, = 1, l A = 0.5, /„, = 0.6, 1, = 0.12, I e = 0.25, S e = 30°. 
Let y = [q 1 , q 2 ] i u = \u t , w 2 ] , x = [(j 1 , q t , q 2 , cj 2 ] , and 



/(*) = 
G(x)- 



AW 



M 



-'"?2 -^(^7i +4l) 

ft 4, o 



£«(*) 8n{ x ) 

gll( X ) gll{ X )_ 



:M" 



Then, the robot system given by (56) can be expressed as 



y = f{x) + G{x)u 



(57) 



which is in the input-output form given by (2). Since the matrix M is positive definite 
(Slotine & Li, 1991), then it is always regular and G(x) = M' 1 is also positive definite. 
The control objective is to force the system outputs q l and q 2 to follow the desired 
trajectories y, n (t) = sin(f) and y d2 (t) = cos(f), respectively. 

To synthesize the direct adaptive fuzzy controller, two fuzzy systems in the form of (12) are 
used to generate the control signals u 1 and u 2 . Each fuzzy system has 

z = re 1 (f),e 1 (f),e 2 (t),e 2 (f)l as input, and for each input variable z , j=l,...,4, three 
Gaussian membership functions are defined as 



A (Z;) = exp 



\(z. +1.25 a2 



0.6 



, /i (z ; ) = exp 



< Ifz^ 



2 0.6 



, ^(z y ) = exp 



-1.25 



>2^ 



0.6 



The robot initial conditions are x(0) = [0.25 0.5 0] , and the initial values of the 
parameter estimates ^(0) and # 2 (0) are set equal to zero. The design parameters used in 
this simulation are chosen as follows: 

Aj = 1, Zj = 1 , K = diag[l,l] , K = diag[5,5] , s = 0.01 , 77 = 5 , and a = 0.001 . 

The simulation results for the first link are shown in Fig. 1, those for the second link are 
shown in Fig. 2, and the control input signals are shown in Fig. 3. We can note that the 
actual trajectories converge to the desired trajectories and the control signals are almost 
smooth. These simulation results demonstrate the tracking capability of the proposed direct 
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adaptive controller and its effectiveness for control tracking of uncertain multivariable 
nonlinear systems. 
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Fig. 1. Tracking curves of link 1: actual (solid lines); desired (dotted lines). 
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Fig. 2. Tracking curves of link 2: actual (solid lines); desired (dotted lines). 
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Fig. 3. Control input signals: u l (solid line); U 2 (dotted line). 
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6. Conclusion 

In this chapter, stable direct and indirect adaptive fuzzy controllers for a class of MIMO 
nonlinear systems with uncertain model dynamics are presented. In the direct scheme, 
fuzzy systems are used to construct adaptively an unknown ideal controller and their 
adjustable parameters are updated by using the gradient descent method in order to 
minimize the error between the unknown controller and the fuzzy controller. In the indirect 
scheme, the controller design is based on the approximation of the system's unknown 
nonlinearities by using fuzzy systems. The free parameters of the used fuzzy systems in this 
case are updated using a gradient descent algorithm that is designed to minimize the 
identification error between the unknown nonlinearities and their adaptive fuzzy 
approximations. Both approaches do not require the knowledge of the mathematical model 
of the plant, guarantee the uniform boundedness of all the signals in the closed-loop system, 
and ensure the convergence of the tracking errors to a neighbourhood of the origin. 
Simulation results for direct adaptive control scheme performed on a two-link robot 
manipulator illustrate the method. Future works will focus on extension of the approach to 
more general MIMO nonlinear systems and its improvement by introducing a state observer 
to provide an estimate of the state vector. 
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1. Introduction 

The traditional control problems of trajectory-tracking and regulation have been extensively 
studied in the field of mobile robotics. In particular, the differential and the omnidirectional 
mobile robots, also known, respectively, as the (2,0) and the (3,0) robots (see (Betourne & 
Campion, 1996), (Campion et al., 1996), have attracted the interest of many control 
researchers. 

It is a common practice in mobile robotics to address control problems taking into account 
only a kinematic representation. In (Canudas et al., 1996) and (Campion et al., 1996) the 
kinematic models for diverse types of mobile robots are presented. 

From a kinematic perspective, the trajectory-tracking control problem of (2,0)-type robot has 
been addressed and solved for example in (D' Andrea-Novel et al., 1992) following a 
dynamic feedback linearization approach. In (Oriolo et al., 2002) a real time implementation 
of a dynamic feedback linearization tracking-controller is presented. For the same class of 
robot, a discrete time approach is considered in (Nino-Suarez et al., 2006) where a path- 
tracking controller based on a sliding mode control technique is presented. 
The regulation and trajectory-tracking problems for the omnidirectional mobile robot (3,0), 
have also received sustained attention. Considering, only its kinematic model, several 
control strategies have been proposed. In (Liu et al., 2003), it is designed a nonlinear 
controller based on a Trajectory Linearization strategy and in (Velasco-Villa et al., 2007), the 
remote control of the (3,0) mobile robot is developed based on a discrete-time strategy 
assuming a time-lag model of the robot. In (Velasco-Villa et al., 2007b) the trajectory- 
tracking problem is solved by means of an estimation strategy that predicts the future 
values of the system based on the exact nonlinear discrete-time model of the robot. 
A more reduced number of contributions have been focused on the dynamic representation 
of the omnidirectional mobile robot. For example, in (Carter et al., 2001), it is described the 
mechanical design of a (3,0) robot and based on its dynamic model it is proposed a PID 
control for each robot wheel. Authors in (Betourne & Campion, 1996) consider an Euler- 
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Lagrange model formulation and present an output feedback controller that solves the 
trajectory-tracking problem. In the same spirit, in (Williams et al., 2002) the dynamic model 
of the mobile robot is considered in order to study the slipping effects between the wheels of 
the vehicle and the working surface. In (Chung et al., 2003), the mobile robot is analyzed in 
the case of a vehicle supporting castor wheels. In (Vazquez & Velasco- Villa, 2008) the 
trajectory tracking problem is addressed and solved by considering a modification of the 
well known Computed-Torque strategy. Finally, in (Kalmar-nagy et al. 2004) the time- 
optimization problem of a desired trajectory is considered for a mobile robot subject to 
admissible input limits in order to obtain feedback laws that are based on the kinematic and 
dynamic models. 

The analysis of Euler-Lagrange systems has produced several feedback strategies that have 
been developed mainly in the area of robot manipulators and, lately, in the area of power 
electronics. In particular, passivity-based control approaches that consider the energy 
managing structure of the system, for instance: the interconnection and damping 
assignment technique developed in (Ortega et al., 2001), and passivity-based approaches 
that exploit the passivity properties of the exact tracking error dynamics and its natural 
passive output are taken in (Ortega et al., 1998), (Sira-Ramirez & Rodriguez-Cortes, 2008), 
(Sira-Ramrez, 2005) and (Sira-Ramrez & Silva-Ortigoza, 2006). 

We address and solve the trajectory-tracking problem of an omnidirectional mobile robot 
taking into account its dynamic model. Contrary to the differential case, the considered 
mobile robot it is not affected by non-holonomics constraints. Following ideas developed in 
the field of power electronics, in this work, we consider a passivity based control technique 
that exploit the passivity properties of the exact tracking error addressed as: Exact Tracking 
Error Dynamics Passive Output Feedback (ETEDPOF), (Sira-Ramrez, 2005), (Sira-Ramrez & 
Silva-Ortigoza, 2006). The performance of the proposed control strategy is contrasted 
through numerical simulations with the well-known Computed-Torque Control (Vazquez & 
Velasco- Villa, 2008) for a desired circular trajectory. 

This paper is organized as follows: Section 2 presents the dynamic model of the 
omnidirectional mobile robot and some structural properties are stated. A brief recall of the 
Computed-Torque solution is also provided. Immediately, in Section 3 the trajectory- 
tracking problem is solved by the Exact Tracking Error Dynamics Passive Output Feedback. 
Closed loop stability is formally proven. In Section 4, the performance of the developed 
strategy is evaluated by means of numerical simulations and compared with the solution 
obtained by the Computed-Torque control technique. Section 5 presents some conclusions. 

2. Omnidirectional Mobile Robot 

A top view of the configuration of a (3,0) mobile robot is depicted in Figure 1. The mobile 
reference frame X m — Y m is located at the center of mass of the vehicle with the X m axis 
aligned with respect to the wheel 3. Wheels 1 and 2 are placed symmetrically with an angle 
(5 = 30° with respect to the Y m axis. The fixed reference frame X — Y provides the absolute 
localization of the vehicle on the workspace. The mobile robot is of the type (Canudas et al., 
1996) (5 m ,8 s ) = (3,0), this is, it has three degrees of mobility and zero degrees of 
steerability allowing the displacements of the vehicle in all directions instantaneously. 
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2.1 Dynamic Model 

Mobile robot velocity expressed in X — Y coordinates is defined as (Campion et al., 1996), 



with, 



q = R T ((/))u 



(i) 



X 




u x 




y 


, u = 


u 2 


, R(4>) = 


W 




w 3 





cos^zS sin(zS 

-siri(zS cos(zi 

1 



y 




X 



Fig. 1. Omnidirectional Mobile Robot 



X 



The point (x,y) is the position of the center of mass of the robot on the plane X — Y and <j> 

is the robot orientation with respect to the X -axis. u x , w 2 are the mobile robot linear 

velocity expressed in the mobile reference system and m 3 is the rotational velocity measured 

in the mobile reference system. 

A simple analysis of the velocity constrains on Figure 1 produces, 
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with, 



JiR(^>)q -J 2 <p = 



(2) 



• sin 8 cos 8 L 

■sin 8 - cos 8 L 

1 L 





r 0" 


, J 2 = 


r 




0;- 



<p = YPy <p 2 <P3 J / where q\, (p 2 , (p-x, represent the angular displacements of wheels one, 
two and three, respectively; 8 is the orientation of the i -wheel with respect to its 
longitudinal axis; L is the distance between the center of each wheel and the center of the 
vehicle and r is the radius of each wheel. 

Following (Campion et al., 1996)-(Canudas et al., 1996), the kinetic energy of the robot is 
given by the wheel rotational energy plus the translational and rotational energy of the 
robot. Therefore, the Lagrangian of the system is obtained as, 



L = \q T R T (<t>)MR(</>)q +\Yjp l T l r <p i , 



(3) 



with M = diag(M ,M ,1 ) and I r = diag(I ,1 ,1 ) . M is the vehicle mass /„ the 
moment of inertia about the Z axis of the vehicle and / is the moment of inertia of each 

wheel about its rotation axis. 

Considering that the kinematics restrictions (2) are satisfied for all t , it is possible to neglect 
the friction and slipping effects between the wheels and the working surface. Then, the 
Euler-Lagrange formulation produces the system representation, 

[M+R T (<f>)E T I r ER(<f>)]q + R T (<f>)E T I r ER(<t>)q = R T {<t>)E T r 

where T — I T l T 2 Z" 3 I , with T t the input torque of each wheel and E = J 2 J\. 
Equivalently 



Dq + C(q)q = Bt, 



(4) 



where, 
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d l 
d { 
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3I..L 1 



with d,=M n + — ' T ,d,=I n + 

p 2r 2 p r 2 



and a '■ 



Mr 

2r 2 



2.2 Structural properties 

In what follows, some structural properties of the dynamic model (4) are stated. These 
properties will be necessary to synthesize the proposed control strategy. 

Property 1 The vector C(q)q does not possess a unique representation, in particular, for the 
development of the feedback law, the following alternative parametrization will be considered, 



f?5 

C{q)q = a -0 



" o 4 y 

-$ -X 

-y x 

Property 2 The structure of matrix C a (q) is such that, 



q = C a {q)q. 



|C fl (<?)||<M<7l 



(5) 



where it is easy to show that k c = — 



2.3 Computed-Torque Control Solution 

Before presenting the main result of the paper we briefly recalled the solution obtained by 
considering a modified version of the well know Computed-Torque control strategy. 
Following (Vazquez & Velasco-Villa, 2008), it is possible to consider for system (4) a 
feedback law of the form, 

BT = Dq d + C(q d )q d -K p q-K d q+C r (q d )q (6) 

where q d is the desired trajectory and q = q — q d is the tracking error. K and K d are 

diagonal and positive definite matrices of proportional and derivative gains and matrix 
C r (q d ) is defined as, 



CMd) = y 



2y d -2x d 



Feedback (6) in closed loop loop with system (4) produces, 



304 Advances in Robot Manipulators 

Dq + C(q)q - C(q d )q d + K p q + K d q - C r (q d )q = 0, 

for which, it is possible to formally proof asymptotic closed loop stability. In what follows, it 
will be presented an alternative feedback control law that solves the same problem. 

3. Control design. 

Consider the following general model of physical systems (Sira-Ramirez et al., 2006; 
Sira-Ramirez et al., 2008). 

Ax = J(x,w)x-R(x,w)x + B(x)w +E(f) 

R T (7) 

y = B x, 

where x is an n dimensional vector, A is a constant symmetric, positive definite matrix, 

J(x, u) is a skew symmetric matrix, R(x, u) is a symmetric positive definite matrix and 

E(f) is a n -dimensional smooth vector function of t or sometimes, a constant vector. The 

input matrix B(x) is a n x m matrix and the output vector y is an m dimensional vector. 

Moreover, we assume that, 

m n 

j(x,«) = j + ^y i jUj + ^y x k x k 

7=1 A=l 

m n 

R(x,u) = R + YP>j + Z R * X * ( 8 ) 

7=1 k=\ 



B(x) = B + ^B /t x i 
Define e u =u—u (t) and the following , 



k- 
k=\ 






(jf-Rfy - (ii-R^y] 

B lX * ■■■ B n x] (9) 

{j?-RrV - (j;;,-r^ s ] 



where x = x (t) . Straightforward computations show that the error dynamics reads as, 
Ae = [j* (x,u,t) -R* (x,u,t))^ + B* (x,i)e u 



y e =B*(x,t) e 
where, 



(10) 
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j*(x, m ,o = J(x,m) + -[p*(0-p*(O t ] 

R*(x,m,?) = R(x, M )--[p*(?) + P*(?) t ] ^ 

B*(x,?) = B(x) + Q*(?), 
with 

P*(f) = M*(0 + L*(0. 

We refer to (10) as the exact open loop error dynamics. 

Following Sira-Ramirez et al., 2006; Sira-Ramirez et al., 2008 we have, 

Assumption 3 Given a feasible smooth bounded reference trajectory x (?) e R , there exists a 
smooth open loop control input u (?) e R , such that for all trajectories starting at x(? ) = x (? ), 
the tracking error e(t) = x(?) - x (?) is identically zero for all t > ? . 

Assumption 4 For any constant positive definite symmetric matrix K the following relation is 
uniformly satisfied 

R*(x,w,?) + B*(x,?)^B*(x,?) T >0. 
Theorem 5 Consider the system (7)-(8) in closed loop with the controller, 

u = u*(t)-KB*(x,t)e. (12) 

Tlien, under Assumptions 3 and 4, the tracking error e(t) is globally asymptotically stabilized to 
zero. 

Proof. Take now the following Lyapunov function candidate, 



V = -e J Ae (13) 



whose time derivative is given by, 



V = —e R (x, u,t)e + e B (x, t)e u . 
Introducing (11) into the above equation, we have 

V = -e T [R'(x,u,t) + B*(x,t)KB*(x,t) J ]e, 
By Assumption 4, the proof is completed. 

3.1 Omnidirectional mobile robot case 

In the following we apply a slightly modified version of the result given in Theorem 5 to 
solve the trajectory-tracking control problem of the omnidirectional mobile robot. For this 
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purpose, we express the dynamic model of the mobile robot (4) in terms of (7). Defining 
initially the feedback, 



it is obtained, 



Consider now the state variables, 



q = x x 



t = B~ x u, 



Dq + C a (q)q = u. 



and a = X7 



v 22 



l 23 



(14) 
(15) 

(16) 



Therefore, system (15) can be rewritten as, 



lj - A 2 



Dt 2 = —C a (x 2 )x 2 + u. 
Consider now a second feedback law of the form, 



(17) 



u = -X[ - Rx 2 + v (18) 

with R=diag{r l ,r 2 ,rT l } . Replacing (18) into (17) we obtain the following closed-loop 
system, 



M A 2 



Dx 2 = -C a (x 2 )x 2 - Xj - Rx 2 + v, 
that can be rewritten in the form of (7) with, 



(19) 



A = 


I 




o" 

D 


5 


R(x,u) = 


"0 0" 
R 



, J(x,w) : 



/ 

-I -C a (x 2 ) 
"0" 



B(x) 



and / a 3 x 3 unity matrix. Now, we obtain the dynamics of the tracking error. Straight 
forward computations show that, 



L*(/) = Q*(0 = 0, 



and 



M*(0: 




C(t) 



(20) 



with 
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C(0 = a 



l 23 



l 22 
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Finally, from equation (11) we have, 



with 



and 
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Hence, the trajectory-tracking error dynamics is described by, 



(21) 



/ 
D 



I J 



22. 







R 



22. 



3.2 Trajectory-tracking solution: Initial proposal 

From the previous developments, it is possible now to state a preliminary solution of the 
trajectory-tracking problem associated with the considered mobile robot. 

Proposition 6 Consider the dynamic model of the omnidirectional mobile robot (4) in closed loop 
with the controller, 

r = 5 _1 [-x 1 -^x 2 +v(0*-^B' i 'e 2 | (22) 

where K =diag{0,K 2 } and K 2 a symmetric positive definite matrix. Assume that x l (t) , 
x 2 (t) and v(t) satisfy Assumption 3 and 



A(R) + A m (K 2 ) 



> jx-, 



(23) 
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Then, the closed-loop system (4)-(22) renders the equilibrium point e x = Xj — Xj = , 
e 2 = x 2 — x 2 = asymptotically stable. 

Proof. The proof of this result can be seen as a particular case of the solution given in the 
next subsection and it is left to the interested reader. 

Remark 7 It is possible to see that by defining controller (22) as a function only of the velocity error 
the convergence of e l is slow. This affects the overall performance of the controller. 

3.3 Main trajectory-tracking solution 

In order to improve the controller convergence produced by the feedback law (22), consider 
now, 

e v = -K x e x -K 2 e 2 (24) 

with K x =diag{k ll ,k l2 ,k li } and K 2 = diag {k 2l ,k 22 , k 2 ^ }' obtaining the closed loop system, 



(25) 
De 2 = -e x + (J 22 - R 22 )e 2 - K x e x — K 2 e 2 ■ 

To show the convergence of the tracking error notice first that (ej,e 2 ) = (0,0) is an 

equilibrium point of system (25). Consider, now a candidate Lyapunov function of the form, 



y ( e \> e i) = ^4 De 2 + -jt\e\ + ee[De 2 +-selK 7 e l . 



(26) 



It is not difficult to see that this function is positive definite for sufficiently small £ . Taking 
the time derivative of equation (26) along (25) it is obtained, 



V = -ee( [i + K x ^ - e\ [tf 22 + K 2 - sd\ 2 
+ ee T x \r 22 -R' 22 )-K x ]: 2 . 
Note now that R 22 can be written as R 22 = R + aF(x 2d ) with, 



(27) 



F(x 2d ) 












-x 



v 22</ 



l 21<7 
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and also, Jxi~^22 can be rewritten as J 22 —R 22 = —R + C a (e 2 ) + aG(x 2d ) with 



G(x 2 d) : 



v 23</ 
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From the fact that, 

||^C*m)||^K<*| and || G C*2rf)|^|W|> 
lengthy but simple computations show that, 

V < -{ [A m (K, ) + s%e x f + [A m (K 2 ) + X m (R 22 ) 

II II II II II |2 

- eX M (D) - ah, 2d \ - m\eA%e 2 \ 

- VsA M (R 22 ) + sa\x 2d I + X M (^)]|ei |e 2 1}. 
Notice now that the above relation can be rewritten as: 



with 



where 



^-Ihl Nik 



[A m (K{) + s\ Pl2 

Pn P22, 



P22 = K ( K 2 ) + K (^22 ) - <&M ( D ) ~ a \\ X 2d I - «a|K |. 
Therefore, the closed-loop system will be stable if the conditions, 

(0 Z m (K l ) + e>0 

(ii) det{P} > 
are satisfied. Condition (?) is trivially satisfied while condition (ii) can be written as a 
second order function of s , that is, 



■y x s +y 2 s + r i >Q 



(28) 



where, 



n 



11 11 1 r 11 n"p 

: &M (P) + «Pl|+ 7LAM (^22) + a \ x 2d\\ 



72 = K ( K z) + K (^22 ) " a \ x 2d I - K ( K i )W (D) + a\\ e A 

- X M (K^)\X U (R 22 ) + a\\x 2d ||J 



n [ 



A m (K l )[A m (K 2 ) + A m (R 22 )-a\\x 2d \i\-U 2 M (K 1 ). 
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It is clear now, from equation (28), that the system will be asymptotically stable for a 
sufficiently small s . Notice that when s — > the required stability condition is reduced to 
^3 > that can be easily obtained by an adequate selection of the control gains together 
with a bounded desired velocity. Hence, we have shown: 

Proposition 8 Consider the dynamic model of the omnidirectional mobile robot (4) in closed loop 
with the controller 



t = B-\-x x -Rx 2 +v{t)*-K iex -K 2 e 2 ] 



(29) 



where K x and K 2 are symmetric positive definite matrices. Assume that x x (t) , x 2 (t) and 
v(t) satisfy Assumption 3 and y^ > . Then, the closed-loop system (4)-(22) renders the 
equilibrium point e x = , e 2 = asymptotically stable. 

4. Numerical simulations 

We carried out numerical simulations to assess the performance of the controller given in 
Proposition 8. The values of the parameters correspond to a laboratory prototype built in 

our institution and they are M p = 9.58 Kg, 7,. = 0.52Kgm 2 , Z = 0.1877m, /- = 0.03812m 
and S = 30° . The initial conditions of the mobile robot are Xj(0) = |0, 0, 1.5 J and 
x 2 (0) = |0, 0, Oj . Finally, the controller parameters are summarized in Table 1. 



Parameter 


Value 


Parameter 


Value 


k u 


200 


^21 


200 


k n 


200 


^22 


200 


^13 


200 


^23 


100 


n, r 2 


200 


r 3 


30 



Table 1. Feedback control law parameters 

It is desired to follow a circular trajectory or radius 0.5 m centered at the origin with initial 
conditions x w (0) = [0. 5, 0, ;r/2j . 

Figure 2 shows the evolution on the plane of the mobile robot when it is considered the 
control strategy proposed on this paper (P) and the one obtained when the Computed- 
Torque control (CT) (6) is considered. The torque input signals are shown on Figure 3 for the 
passive approach and on Figure 4 for the Computed-Torque control. It is clear that by 
selecting the control gains under the restriction of equivalent torque magnitude, the control 
strategy proposed in this work has a better performance than the one obtained by the 
Computed-Torque scheme. Finally, the evolution of the position and velocity errors for the 
passivity control strategy are shown on Figures 5 and 6 respectively. 
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Fig. 2. Evolution on the plane of the mobile robot 
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Fig. 3. Evolution of the applied torque for the passive strategy. 
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Fig. 4. Evolution of the applied torque for the Computed-Torque strategy. 
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Fig. 5. Evolution of the position errors. 
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Fig. 6. Velocity errors. 
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5. Conclusions 

The trajectory-tracking problem for the omnidirectional mobile robot considering its 
dynamic model has been addressed and solved by means of a full state information time 
varying feedback based on a methodology that exploits the passivity properties of the exact 
tracking error dynamics. The asymptotic stability of the closed loop system is formally 
proved. Numerical simulations are proposed to illustrate the properties of the closed-loop 
system showing a better performance than the control obtained by the well known 
Computed-Torque approach. 
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1. Introduction 



The purpose of this paper is to describe a concept of eclecticism for the design, development, 
simulation and implementation of a real time controller for an intelligent, vision guided 
robot or robots. The use of an eclectic perceptual, creative controller that can select its own 
tasks and perform autonomous operations is illustrated. This eclectic controller is a new 
paradigm for robot controllers and is an attempt to simplify the application of intelligent 
machines in general and robots in particular. The idea is to uses a task control center and 
dynamic programming approach. However, the information required for an optimal 
solution may only partially reside in a dynamic database so that some tasks are impossible 
to accomplish. So a decision must be made about the feasibility of a solution to a task before 
the task is attempted. Even when tasks are feasible, an iterative learning approach may be 
required. The learning could go on forever. The dynamic database stores both global 
environmental information and local information including the kinematic and dynamic 
models of the intelligent robot. The kinematic model is very useful for position control and 
simulations. However, models of the dynamics of the manipulators are needed for tracking 
control of the robot's motions. Such models are also necessary for sizing the actuators, 
tuning the controller, and achieving superior performance. Simulations of various control 
designs are shown. Much of the model has also been used for the actual prototype Bearcat 
Cub mobile robot. This vision guided robot was designed for the Intelligent Ground Vehicle 
Contest. A novel feature of the proposed approach lies in the fact that it is applicable to both 
robot arm manipulators and mobile robots such as wheeled mobile robots. This generality 
should encourage the development of more mobile robots with manipulator capability since 
both models can be easily stored in the dynamic database. The multi task controller also 
permits wide applications. The use of manipulators and mobile bases with a high-level 
control are potentially useful for space exploration, manufacturing robots, defense robots, 
medical robotics, and robots that aid people in daily living activities. 

An important question in the application of intelligent machines is: can a major paradigm 
shift can be effected from industrial robots to a more generic service robot solution? That is, 
can we perform an eclectic design? (Hall, et al. 2007) 
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The purpose of this paper is to examine the theory of robust learning for intelligent 
machines. A main question in the application of intelligent machines is: can a major 
paradigm shift can be effected? 

Eclecticism as defined by Wikipedia as " a conceptual approach that does not hold rigidly to a single 
paradigm or set of assumptions, but instead draws upon multiple theories, styles, or ideas to gain 
complementary insights into a subject, or applies different theories in particular cases. " 
http://en.wikipedia.org/wiki/Eclecticism 

A scientific paradigm had been defined by Kuhn as "answers to the following key questions: 

• what is to be observed and scrutinized, 

• what kind of questions are supposed to be asked and probed for answers in 
relation to this subject, 

• how are these questions are to be structured, 

• how should the results of scientific investigations be interpreted. 

• how is an experiment to be conducted, and what equipment is available to conduct 
the experiment. 

"Thus, within normal science, the paradigm is the set of exemplary experiments that are likely to be 
copied or emulated. The prevailing paradigm often represents a more specific way of viewing reality, 
or limitations on acceptable programs for future research, than the much more general scientific 
method. " 

In the eclectic control, some answers to the key questions are: 

• 77k performance of the intelligent machine will be observed 

• Actual or simulated behaviors will lead to questions of normal or useful responses 

• Questions should be structured to permit answers from queries of the database 

• Objectively by anyone in the world 

• Simulations are much more cost effective than actual performance tests 

The proposed theory for eclectic learning is also based on the previous perceptual creative 
controller for an intelligent robot that uses a multi- modal adaptive critic for performing 
learning in an unsupervised situation but can also be trained for tasks in another mode and 
then is permitted to operate autonomously. The robust nature is derived from the automatic 
changing of task modes based on a dynamic data base and internal measurements of error at 
appropriate locations in the controller. 

The eclectic controller method is designed for complex real world environments. However, 
analysis and simulation is needed to clarify the decision processes and reduce the danger in 
real world operations. 

The eclectic controller uses a perceptual creative learning architecture to integrate a Task 
Control Center (TCC) and a dynamic database (DD) with adaptive critic learning algorithms 
to permit these solutions. Determining the tasks to be performed and the data base to be 
updated are the two key elements of the design. These new decision processes encompass 
both decision and estimation theory and can be modeled by neural networks and 
implemented with multi-threaded computers. 

The main thrust of this paper is to present the eclectic theory of learning that can be used for 
developing control architectures for intelligent machines. Emphasis will be placed on the 
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missing key element, the dynamic data base, since the control architectures for neural 
network control of vehicles in which the kinematic and dynamic models are known but one 
or more parameters must be estimated is a simple task that has been demonstrated. 
The mathematical models for the kinematics and dynamics were developed and the main 
emphasis was to explore the use of neural network control and demonstrate the advantages 
of these learning methods. The results indicate the method of solution and its potential 
application to a large number of currently unsolved problems in complex environments. 
The adaptive critic neural network control is an important starting point for future learning 
theories that are applicable to robust control and learning situations. 

The general goal of this research is to further develop an eclectic theory of learning that is 
based on human learning but applicable to machine learning and to demonstrate its 
application in the design of robust intelligent systems. To obtain broadly applicable results, 
a generalization of adaptive critic learning called Creative Control (CC) for intelligent robots 
in complex, unstructured environments has been used. The creative control learning 
architecture integrates a Task Control Center (TCC) and a Dynamic Knowledge Database 
(DKD) with adaptive critic learning algorithms. 

Recent learning theories such as the adaptive critic have been proposed in which a critic 
provides a grade to the controller of an action module such as a robot. The creative control 
process which is used is "beyond the adaptive critic." 

A mathematical model of the creative control process is presented that illustrates the use for 
mobile robots. 

1.1 Dynamic Programming 

The intelligent robot in this paper is defined as a decision maker for a dynamic system that 
may make decisions in discrete stages or over a time horizon. The outcome of each decision 
may not be fully predictable but may be anticipated or estimated to some extent before the 
next decision is made. Furthermore, an objective or cost function can be defined for the 
decision. There may also be natural constraints. Generally, the goal is to minimize this cost 
function over some decision space subject to the constraints. With this definition, the 
intelligent robot can be considered as a set of problems in dynamic programming and 
optimal control as defined by Bertsekas (Bertsekas, 2000). 

Dynamic programming (DP) is the only approach for sequential optimization applicable to 
general nonlinear and stochastic environments. However, DP needs efficient approximate 
methods to overcome its dimensionality problems. It is only with the presence of artificial 
neural network (ANN) and the invention of back propagation that such a powerful and 
universal approximate method has become a reality. 

The essence of dynamic programming is Bellman's Principle of Optimality . (White and Sofge, 1992) 
"An optimal policy has the property that whatever the initial state and initial decision are, the 
remaining decisions must constitute an optimal policy with regard to the state resulting from the first 
decision" (Bertsekas, 2000) (p. 83). 

The original Bellman equation of dynamic programming for adaptive critic algorithm may 
be written as shown in Eq (1): 

J(R(t)) = max(U(R(t),u(t))+ < J(R(t + 1)) >) /(l + r)-U (1) 

u(t) 
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Where R(t) is the model of reality or state form, U( R(t),u(t)) is the utility function or local 

cost, u(t) is the action vector, J(R(t)) is the criteria or cost-to-go function at time t, r and Uo 

are constants that are used only in infinite-time-horizon problems and then only sometimes, 

and where the angle brackets refer to expected value. 

The user provides a utility function, U, and a stochastic model of the plant, R, to be 

controlled. The expert system then tries to solve the Bellman equation for the chosen model 

and utility function to achieve the optimum value of J by picking the action vector u(t). If an 

optimum J cannot be determined, an approximate or estimate value of the J function is used 

to obtain an approximate optimal solution. 

Regarding the finite horizon problems, which we normally try to cope with, one can use Eq (2): 

J(R(t)) = max(U(R(t),u(t))+ < AW + 1)) >) /(l + r) (2) 

u(t) 

Dynamic programming gives the exact solution to the problem of how to maximize a utility 
function U(R(t), u(t)) over the future times, t, in a nonlinear stochastic environment. 
Dynamic programming converts a difficult long-term problem in optimization over time 
<U(R(t))>, the expected value of U(R(t)) over all the future times, into a much more 
straightforward problem in simple, short-term function maximization - after we know the 
function J. Thus, all of the approximate dynamic programming methods discussed here are 
forced to use some kind of general-purpose nonlinear approximation to the J function, the 
value function in the Bellman equation, or something closely related to J(Werbos, 1999). 

In most forms of adaptive critic design, we approximate J by using a neural network. 
Therefore, we approximate J(R) by some function J(R, W) , where W is a set of weights or 

parameters, J is called a critic network (Widrow, et al., 1973) 

If the weights W are adapted or iteratively solved for, in real time learning or offline 
iteration, we call the Critic an Adaptive Critic (Werbos, 1999). 

An adaptive critic design (ACD) is any system which includes an adapted critic component; 
a critic, in turn, is a neural net or other nonlinear function approximation which is trained to 
converge to the function J(X). 

In adaptive critic learning or designs, the critic network learns to approximate the cost-to-go 
or strategic utility function J and uses the output of an action network as one of its' inputs, 
directly or indirectly. When the critic network learns, back propagation of error signals is 
possible along its input feedback to the action network. To the back propagation algorithm, 
this input feedback looks like another synaptic connection that needs weights adjustment. 
Thus, no desired control action information or trajectory is needed as supervised learning. 

2. Adaptive Critic And Creative Control 

Most advanced methods in neurocontrol are based on adaptive critic learning techniques 
consisting of an action network, adaptive critic network, and model or identification 
network as show in Figure 1, These methods are able to control processes in such a way, 
which is approximately optimal with respect to any given criteria taking into consideration 
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of particular nonlinear environment. For instance, when searching for an optimal trajectory 
to the target position, the distance of the robot from this target position can be used as a 
criteria function. The algorithm will compute the proper steering, acceleration signals for 
control of vehicle, and the resulting trajectory of the vehicle will be close to optimal. During 
trials (the number depends on the problem and the algorithm used) the system will improve 
performance and the resulting trajectory will be close to optimal. The freedom of choice of 
the criteria function makes the method applicable to a variety of problems. The ability to 
derive a control strategy only from trial/ error experience makes the system capable of 
semantic closure. These are very strong advantages of this method. 




t -|w[ 



criu?rial 
function 



Structure of the adaptive critic controller based on artificial 
neural networks. 

Fig. 1. Structure of the adaptive critic controller (Jaska and Sine, 2000) 



Creative Learning Structure 

It is assumed that we can use a kinematic model of a mobile robot to provide a simulated 
experience to construct a value function in the critic network and to design a kinematic 
based controller for the action network. A proposed diagram of creative learning algorithm 
is shown in Figure 2 (Jaska and Sine, 2000). In this proposed diagram, there are six 
important components: the task control center, the dynamic knowledge database, the critic 
network, the action network, the model-based action and the utility funtion. Both the critic 
network and action network can be constructed by using any artificial neural networks with 
sigmoidal function or radial basis function (RBF). Furthermore, the kinematic model is also 
used to construct a model-based action in the framework of adaptive critic-action approach. 
In this algorithm, dynamic databases are built to generalize the critic network and its 
training process and provide evironmental information for decision making. It is especially 
critical when the operation of mobile robots is in an unstructured environments. 
Furthermore, the dynamic databases can also used to store environmental parameters such 
as Global Position System (GPS) way points, map information, etc. Another component in 
the diagram is the utility function for a tracking problem (error measurement). In the 
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diagram, Xk, Xkd, Xkd+i are inputs and Y is the ouput and J(t), J(t+1) is the critic function at 
the time. 



Criteria filters 



Adaptive critic learning system 




J(t+1) Xdk+1 







Model- 
based 
Action 



O 



Fig. 2. Proposed Creative Learning Algorithm Structure 



Dynamic Knowledge Database (DKD) 

The dynamic databases contain domain knowledge and can be modified to permit 
adaptation to a changing environment. Dynamic knowledge databases may be called a 
"neurointerface" (Widrow and Lamego, 2002) in a dynamic filtering system based on neural 
networks (NNs) and serves as a "coupler" between a task control center and a nonlinear 
system or plant that is to be controlled or directed. The purpose of the coupler is to provide 
the criteria functions for the adaptive critic learning system and filter the task strategies 
commanded by the task control center. The proposed dynamic database contains a copy of 
the model (or identification). Action and critic networks are utilized to control the plant 
under nominal operation, as well as make copies of a set of parameters (or scenario) 
previously adapted to deal with a plant in a known dynamic environment. The database 
also stores copies of all the partial derivatives required when updating the neural networks 
using backpropagation through time (Yen and Lima, 2002). The dynamic database can be 
expanded to meet the requirements of complex and unstructured environments. 
The data stored in the dynamic database can be uploaded to support offline or online 
training of the dynamic plant and provide a model for identification of nonlinear dynamic 
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environment with its modeling function. Another function module of the database 
management is designed to analyze the data stored in the database including the sub-task 
optima, pre-existing models of the network and newly added models. The task program 
module is used to communicate with the task control center. The functional structure of the 
proposed database management system (DBMS) is shown in Figure 3. The DBMS can be 
customized from an object-relational database. 

In existing models the database is considered to be static. The content of the data base may 
be considered as information. However, our experience with the World Wide Web is that 
the "information" is dynamic and constantly changing and often wrong. 




Fig. 3. Functional structure of dynamic database 



2.3 Task Control Center (TCC) 

The task control center (TCC) can build task-level control systems for the creative learning 
system. By "task-level", we mean the integration and coordination of perception, planning and 
real-time control to achieve a given set of goals (tasks) (Lewis, et al., 1999). TCC provides a 
general task control framework, and it is to be used to control a wide variety of tasks. Although 
the TCC has no built-in control functions for particular tasks (such as robot path planning 
algorithms), it provides control functions, such as task decomposition, monitoring, and resource 
management, that are common to many applications. The particular task built-in rules or criteria 
or learning J functions are managed by the dynamic database controlled with TCC to handle the 
allocation of resources. The dynamic database matches the constraints on a particular control 
scheme or sub-tasks or environment allocated by TCC. 

The task control center acts as a decision-making system. It integrates domain knowledge or 
criteria into the database of the adaptive learning system. According to Simmons (Simmons, 
2002), the task control architecture for mobile robots provides a variety of control constructs that 
are commonly needed in mobile robot applications, and other autonomous mobile systems. The 
goal of the architecture is to enable autonomous mobile robot systems to easily specify 
hierarchical task-decomposition strategies, such as how to navigate to a particular location, or 
how to collect a desired sample, or how to follow a track in an unstructured environment. This 
can include temporal constraints between sub-goals, leading to a variety of sequential or 
concurrent behaviors. TCC schedules the execution of planned behaviors, based on those 
temporal constraints acting as a decision-making control center. 
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Integrating the TCC with the adaptive critic learning system and interacting with the dynamic 
database, the creative learning system provides both task-level and real-time control or learning 
within a single architectural framework. Through interaction with human beings to attain the 
input information for the system, the TCC could decompose the task strategies to match the 
dynamic database for the rules of sub-tasks by constructing a distributed system with flexible 
mechanisms, which automatically provide the right data at the right time. The TCC also provides 
orderly access to the resources of the dynamic database with built-in learning mechanisms 
according to a queue mechanism. This is the inter-process communication capability between the 
task control center and the dynamic database. The algorithm on how to link the task control 
center and the dynamic database is currently done by the human designers. 



Creative learning controller for intelligent robot control 

Creative learning may be used to permit exploration of complex and unpredictable 
environments, and even permit the discovery of unknown problems, ones that are not yet 
recognized but may be critical to survival or success. By learning the domain knowledge, the 
system should be able to obtain the global optima and escape local optima. The method attempts 
to generalizes the highest level of human learning - imagination. As a ANN robot controller, the 
block diagram of the creative controller can be presented in Figure 4. 

Experience with the guidance of a mobile robot has motivated this study and has progressed 
from simple line following to the more complex navigation and control in an unstructured 
environment. The purpose of this system is to better understand the adaptive critic learning 
theory and move forward to develop more human-intelligence-like components into the 
intelligent robot controller. Moreover, it should extend to other applications. Eventually, 
integrating a criteria knowledge database into the action module will develop a powerful 
adaptive critic learning module. 



Creative 
Controller 




Robot 



Sensors 



> 



Fig. 4. Block diagram of creative controller 
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A creative controller is designed to integrate domain knowledge or criteria database and the 

task control center into the adaptive critic neural network controller. It provides a needed 

and well-defined structure for autonomous mobile robot application. In effect, it replaces a 

human doing remote control. We have used the intelligent mobile robot as the test-bed for 

the creative controller. 

The task control center of the creative learning system can be considered hierarchically as 

follows: 

♦ Mission for robot - e.g. mobile robot 

* Task for robot to follow - J : task control 
♦ Track for robot to follow 

♦ Learn non-linear system model- model discovery 
* Learn unknown parameters 
Adaptive Critic system Implementation 
Adaptive Critic system and NN 

In order to develop the creative learning algorithm addressed above, we have taken a 
bottom-up approach to implement adaptive critic controllers by first using neural network 
for on-line or off-line learning methods. 16 Then the proposed dynamic knowledge database 
and task control center are added with some to be realized in future research projects. 

Tuning algorithm and stability analysis 

For linear time invariant systems it is straightforward to examine stability by investigating 
the poles in the s-plane. However, stability of a nonlinear dynamic systems is much more 
complex, thus the stability criteria and tests are much more difficult to apply than those for 
linear time invariant systems 17 " 19 . For general nonlinear continuous time systems, the state 
space model is 

x = f[x(t),u(t)] 

y = g[x(t),u(t)] (3) 

where the nonlinear differential equation is in state variable form, x(t) is the state vector and 
u(t) is the input and the second equation y(t) is the output of the system. 

Creative controller and nonlinear dynamic system 

For a creative controller, the task control center and the dynamic database are not time- 
variable systems; therefore, the adaptive critic learning component determines the stability 
of the creative controller. As it is discussed in the previous section, the adaptive critic 
learning is based on critic and action network designs, which are originated from artificial 
neural network (ANN), thus stability of the system is determined by the stability of the 
neural networks (NN) or convergence of the critic network and action network training 
procedure. 

The creative controller is a nonlinear system. It is not realistic to explore all the possibilities 
of the nonlinear systems and prove that the controller is in a stable state. We have used both 
robot arm manipulators and mobile robot models to examine a large class of problems 
known as tracking in this study. The objective of tracking is to follow a reference trajectory 
as closely as possible. This may also be called optimal control since we optimize the tracking 
error over time. 
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Critic and Action NN Weights Tuning Algorithm 

In adaptive critic learning controller, both the critic network and action network use 
multilayer NN. Multilayer NN are nonlinear in the weights V and so weight tuning 
algorithms that yield guaranteed stability and bounded weights in closed-loop feedback 
systems have been difficult to discover until a few years ago. 



3. Some Eclectic Control Scenarios 

Urban Rescue Scenarios 

Suppose a mobile robot is used for urban rescue as shown in Figure 5. It waits at a start 
location until a call is received from a command center. Then it must go rescue a person. 
Since it is in an urban environment, it must use the established roadways. Along the 
roadways, it can follow pathways. However, at intersections, it must choose between 
various paths to go to the next block. Therefore, it must use a different criteria at the corners 
than along the track. The overall goal is to arrive at the rescue site with minimum time. To 
clarify the situations consider the following steps. 

1. Start location - the robot waits at this location until it receives a task command to 
go to a certain location. 

2. Along the path, the robot follows a road marked by lanes. It can use a minimum 
mean square error between its location and the lane location during this travel. 

3. At intersections, the lanes disappear but a database gives a GPS waypoint and the 
location of the rescue goal. 

This example requires the use of both continuous and discrete tracking, a database of known 
information and multiple criteria optimization. It is possible to add a large number of real- 
world issues including position estimation, perception, obstacles avoidance, communication, 
etc. 
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Fig. 5. Simple urban rescue site 
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In an unstructured environment as shown in Figure 5, we assume that information collected 
about different potions of the environment could be available to the mobile robot, 
improving its overall knowledge. As any robot moving autonomously in this environment 
must have some mechanism for identifying the terrain and estimating the safety of the 
movement between regions (blocks), it is appropriate for a coordination system to assume 
that both local obstacle avoidance and a map-building module are available for the robot 
which is to be controlled. The most important module in this system is the adaptive system 
to learn about the environment and direct the robot action. 18 

A Global Position System (GPS) may be used to measure the robot position and the distance 
from the current site to the destination and provide this information to the controller to 
make its decision on what to do at next move. The GPS system or other sensors could also 
provides the coordinates of the obstacles for the learning module to learn the map, and then 
aid in avoiding the obstacles when navigating through the intersections A, B or G, D to 
destination T. 

Task control center 

The task control center (TCC) acts a decision-making command center. It takes 
environmental perception information from sensors and other inputs to the creative 
controller and derives the criteria functions. We can decompose the robot mission at the 
urban rescue site shown as Figure 5 into sub- tasks as shown in Figure 6. Moving the robot 
between the intersections, making decisions is based on control-center-specified criteria 
functions to minimize the cost of mission. It's appropriate to assume that Jl and J2 are the 
criteria functions that the task control center will transfer to the learning system at the 
beginning of the mission from the Start point to Destination (T). Jl is a function of t related 
to tracking error. J2 is to minimize the distance of the robot from A to T since the cost is 
directly related to the distance the robot travels. 

• From Start (S) to intersection A: robot follow the track SA with the Jl as objective 
function 

• From intersection A to B or D: which one will be the next intersection, the control 
center takes both Jl and J2 as objective functions. 
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Fig. 6. Mission decomposition diagrams 



Dynamic databases 

Dynamic databases would store task-oriented environment knowledge, adaptive critic 
learning parameters and other related information for accomplishing the mission. In this 
scenario, the robot is commanded to reach a dangerous site to conduct a rescue task. The 
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dynamic databases saved a copy of the GPS weight points S, A, B, C, D, E, F, G and T. The 
map for direction and possible obstacle information is also stored in the dynamic databases. 
A copy of the model parameters can be saved in the dynamic database as shown in the 
simplified database Figure 7. The action model will be updated in the dynamic database if 
the current training results are significantly superior to the previous model stored in the 
database. 



Database fields 


Field Description 


MODELJD 


Action model ID 


MODEL_NAME 


Action model name 


UTILITY_FUN 


Utility function 


CRITERIA_FUN 


Criteria function 




Adaptive Critic Training Parameters 


INPUT_CRITIC 


Input to critic network 


DELTJ 


J(t+1)-J(t) 


|... 



Fig. 7. Semantic dynamic database structure 

Robot Learning Module 

Initial plans such as road tracking and robot navigating based on known and assumed 
information, can be used to incrementally revise the plan as new information is discovered 
about the environment. The control center will create criteria functions according to the 
revised information of the world through the user interface. These criteria functions along 
with other model information of the environment will be input to the learning system. There 
is a data transfer module from the control center to the learning system as well as a module 
from the learning system to the dynamic database. New knowledge is used to explore and 
learn, training according to the knowledge database information and then decide which to 
store in the dynamic database and how to switch the criteria. The simplest style in the 
adaptive critic family is heuristic dynamic programming (HDP). This is NN on-line adaptive 
critic learning. There is one critic network, one action network and one model network in 
the learning structure. U(t) is the utility function. R is the critic signal as J (criteria function). 
The learning structure and the parameters are saved a copy in the dynamic database for the 
system model searching and updating. 



Other Demonstrations 

The UC Robot Team is attempting to exploit its many years of autonomous ground vehicle 
research experience to demonstrate its capabilities for designing and fabricating a smart 
vehicle control for unmanned systems operation as shown in Figures 8, 9 and 10. The 
purpose of this research is to perform a proof by demonstration through system design and 
integration of a new autonomous vehicle that would integrate advanced technologies in 
Creative Control with advanced autonomous robotic systems. 
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The main thrust of our effort is the intelligent control software which provides not only 
adaptation but also learning and prediction capabilities. However, since a proof by 
demonstration is needed, further efforts in simulation and implementation are necessary. 
This new Creative Control has been developed over the past several years and has been the 
subject of many UC dissertations and papers. 




Fig. 8. Bearcat Cub intelligent vehicle designed for IGVC 

■■■ 




Fig. 9. NAC Jeep prototype at UC 




Fig. 10. Hybrid Vehicle 
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4. CONCLUSIONS AND RECOMMENDATIONS 

The eclectic control is proposed and described as a general perceptual creative adaptive 
critic learning system. The task control center is a decision-making command center for the 
intelligent creative learning system. The dynamic knowledge database integrates task 
control center and adaptive critic learning algorithm into one system. It also provides a 
knowledge domain for the task command center to perform decision-making. Furthermore, 
creative learning can be used to explore complex and unpredictable environments, and even 
permit the discovery of unknown problems. By learning the domain knowledge, the system 
should be able to obtain the global optima and escape local optima. The challenge is now in 
implementing such concepts in practical applications. 
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Abstract 

The chapter focuses on the enhanced stiffness modeling and analysis of serial kinematic 
chains with passive joints, which are widely used in parallel robotic systems. In contrast to 
previous works, the stiffness is evaluated for the loaded working mode corresponding to the 
static equilibrium of the elastic forces and the external wrench acting upon the manipulator 
end point. It is assumed that the manipulator elasticity is described by a multidimensional 
lumped-parameter model, which consists of a chain of rigid bodies connected by 6-dof 
virtual springs. Each of these springs characterize flexibility of the corresponding link or 
actuating joint and takes into account both their translational/ rotational compliance and the 
coupling between them. The proposed technique allows finding the full-scale "load- 
deflection" relation for any given workspace point and to linearise it taking into account 
variation of the manipulator Jacobian due to the external load. These enable evaluating 
critical forces that may provoke non-linear behavior of the manipulator, such as sudden 
failure due to elastic instability (buckling). The advantages of the developed technique are 
illustrated by several examples that deal with kinematic chains employed in typical parallel 
manipulators. 

Keywords 

Stiffness model, external loading, kinetostatic analysis, passive joints, buckling, divergence 
of equilibrium, static stability 

1. Introduction 

Due to the increasing industrial needs, novel approaches in mechanical design of robotic 
manipulators are targeted at essential reduction of moving masses and achieving high 
dynamic performances with relatively low energy consumption. This motivates using 
advanced kinematical architectures and light-weight materials, as well as minimization of 
the cross-sections of all manipulator elements (Siciliano & Khatib, 2008). The primary 
constraint for such minimization is the mechanical stiffness of the manipulator, which must 
be evaluated taking into account external disturbances (loading) imposed by a relevant 
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manufacturing process. However, in robotic literature, the manipulator stiffness is usually 
evaluated by a linear model, which defines the static response to the external force/ torque, 
assuming that the compliant deflections are small and the external loading is insignificant 
(Zhang et al., 2009; Majou et al., 2007). At the same time, in many practical applications 
(such as milling, for instance), the loading is essential and conventional stiffness modeling 
techniques must be used with great caution (Los et al., 2008). Moreover, for the 
manipulators with light-weight links, there is a potential danger of buckling phenomena 
that is known from general theory of elastic stability (Timoshenko & Goodier, 1970). Hence, 
the existing stiffness modeling techniques for high-performance robotic manipulators must 
be revised and enhanced, in order to add ability of detecting non-linear effects and avoid 
structural failures caused by the loading. 

The existing approaches for the manipulator stiffness modeling may be roughly divided into 
three main groups: the Finite Element Analysis (FEA) (Piras et al., 2005; Hu et al., 2007; 
Nagai & Liu 2007), the matrix structural analysis (SMA) (Deblaise et al. 2006, Martin, 1966, 
Li et al., 2002), and the virtual joint method (VJM) that is often called the lumped modeling 
(Gosselin, 1990; Pashkevich et. al. 2008; Quennouelle & Gosselin 2008 a). The most accurate 
of them is the Finite Element Analysis, which allows modeling links and joints with its true 
dimension and shape. However it is usually applied at the final design stage because of the 
high computational expenses required for the repeated remeshing of the complicated 3D 
structure over the whole workspace. The SMA also incorporates the main ideas of the FEA, 
but operates with rather large elements - 3D flexible beams that are presented in the 
manipulator structure. This leads obviously to the reduction of the computational expenses, 
but does not provide clear physical relations required for the parametric stiffness analysis. 
And finally, the VJM method is based on the expansion of the traditional rigid model by 
adding the virtual joints (localized springs), which describe the elastic deformations of the 
links, joints and actuators (Salisbury, 1980; Gosselin, 1990). The VJM technique is widely 
used at the pre-design stage and will be extended in this paper for the case of the preloaded 
manipulators. 

It should be noted, that there are a number of variations and simplifications of the VJM, 
which differ in modeling assumptions and numerical procedures. Recent modification of 
this method allows to extend it to the over-constrained manipulator and to apply it at any 
workspace point, including the singular ones (Pashkevich et. al. 2009 a, b). Besides, to take 
into account real shape of the manipulator components, the stiffness parameters may be 
evaluated using the FEA modeling. The latter provided the FEA-accuracy throughout the 
whole workspace without exhaustive remeshing required for the classical FEA. 
At present, there is very limited number of publication that directly addressed the problem 
of the stiffness modeling for loaded manipulators. The most essential results were obtained 
in (Alici, & Shirinzadeh; 2005; Quennouelle & Gosselin, 2008 b; Kovecses & Angeles, 2007) 
where the stiffness matrix was computed taking into account the change in the manipulator 
configuration due to the preloading. However, the problem of finding the corresponding 
loaded equilibrium was omitted, so the Jacobian and Hessian were computed in a 
traditional way, i.e. for the neighborhood of the unloaded equilibrium. The latter yielded 
essential computational simplification but also imposed crucial limitations, not allowing 
detecting the buckling and other non-liner effects. 

This chapter focuses on the stiffness modeling of serial kinematic chains with passive joints, 
which are widely used in parallel robotic systems. It presents an enhanced solution of the 
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considered problem, taking into account influence of the external force/ torque on the 
manipulator configuration as well as change in the Jacobian due to the external loading. It 
implements the virtual joint technique that describes the compliance of the manipulator 
elements by a set of localized six-dimensional springs separated by rigid links and perfect 
joints. In contrast to previous works, the developed technique allows to obtain the full-scale 
"load-deflection" relation for any given workspace point and to compute the desired matrix 
for any manipulator configuration (including singular ones), implicitly taking into account 
the kinematic redundancy imposed by the passive joints. Besides, it enables designer to 
evaluate critical forces that may provoke non-linear manipulator behaviour, such as sudden 
failure due to elastic instability (buckling) which has not been previously studied in robotic 
literature. Another contribution is a numerical algorithm for computing the loaded 
equilibrium and its analytical criteria for its stability analysis. 

The remainder of the chapter is organized as follows. Section 2 defines the research problem 
and basic assumptions. In Section 3, it is proposed a numerical algorithm for computing of 
the loaded static equilibrium and its stability analysis. Section 4 focuses on the stiffness 
matrix evaluation taking into account external loading and presence of passive joints. 
Section 5 contains a set of illustrative examples that demonstrate possible nonlinear 
behavior of loaded serial kinematic chains. And finally, Section 6 summarizes the main 
results and contributions. 



2. Problem of Stiffness modelling 

2.1 Manipulator Architecture 

Let us consider a general serial kinematic chain, which consists of a fixed "Base", a number 
of flexible actuated joints "Ac", a serial chain of flexible "Links", a number of passive joints 
"Ps" and a moving "Platform" at the end of the chain (Fig. 1). It is assumed that all links are 
separated by the joints (actuated or passive, rotational or translational) and the joint type 
order is arbitrary. Besides, it is admitted that some links may be separated by actuated and 
passive joints simultaneously. Such architecture can be found in most of parallel 
manipulators (Fig. 2) where several similar kinematic chains are connected to the same base 
and platform in a different way (with rotation of 90° or 120°, for instance), in order to 
eliminate the redundancy caused by the passive joints. It is obvious that such kinematic 
chains are statically under-constrained and their stiffness analysis can not be performed by 
direct application of the standard methods. 

Typical examples of the examined kinematic chains can be found in 3-PUU translational 
parallel kinematic machine (Li & Xu, 2008), in Delta parallel robot (Clavel, 1988) or in 
parallel manipulators of the Orthoglide family (Chablat & Wenger, 2003) and other 
manipulators (Merlet, 2006). It worth mentioning that here a specific spatial arrangement of 
under-constrained chains yields the over-constrained mechanism that posses a high structural 
rigidity with respect to the external force. In particular, for Orthoglide, each kinematic chain 
prevents the platform from rotating about two orthogonal axes and any combination of two 
kinematic chains suppresses all possible rotations of the platform. Hence, the whole set of 
three kinematic chains produces non-singular stiffness matrix while for each separate chain 
the stiffness matrix is singular. This motivates development of dedicated stiffness analysis 
techniques that are presented below. 
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Fig. 1. General serial kinematic chain and its VJM model (Ac - actuated joint, Ps - passive 
joint). 
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Fig. 2. Architecture of typical parallel manipulators and their kinematics chains 



2.2 Basic Assumptions 

To evaluate the stiffness of the considered serial manipulator, let us apply a modification of 
the virtual joint method (VJM), which is based on the lump modeling approach (Gosselin, 
1990). According to this approach, the original rigid model should be extended by adding 
virtual joints (localized springs), which describe elastic deformations of the links. Besides, 
virtual springs are included in the actuating joints, to take into account the stiffness of the 
control loop. Under such assumptions, the kinematic chain can be described by the 
following serial structure: 

(a) a rigid link between the manipulator base and the first actuating joint described by the 
constant homogenous transformation matrix T Base ; 

(b) the 6-d.o.f. actuating joints defining three translational and three rotational actuator 
coordinates, which are described by the homogenous matrix function T^fBIJ where 

8' = (8'", 8'", 8°', 8°' , 8°' , 6'" ) are the virtual spring coordinates; 

(c) the 6-d.o.f. passive joints defining three translational and three rotational passive joins 
coordinates, which are described by the homogenous matrix function T 3fl (q' ; ) where 

1 , = (<7x> <?v> <?'> <?!,> <?L> %-) are *he passive joint coordinates; 

(d) the rigid links, which are described by the constant homogenous transformation matrix 
T. ; 

(e) a 6-d.o.f. virtual joint defining three translational and three rotational link-springs, which 
are described by the homogenous matrix function T 3D (8^ M |, where 
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8^ = (9' v , e' v , 9', e; r , 9^, e; pz ) , (0' v , 0',., 0;) and (e 1 ^, e; pv , #\ correspond to the elementary 

translations and rotations respectively; 

(f) a rigid link from the last link to the end-effector, described by the homogenous matrix 

transformation T Tool . 

In the frame of these notations, the final expression defining the end-effector location subject 
to variations of all joint coordinates of a single kinematic chain may be written as the 
product of the following homogenous matrices 



T = T 



■ n( T 3 D K ) ■ T 3s (C ) ■ T L t ■ T 3fl (Ok* ) ■ T 3fl (q;' )) ■ T Tool (1) 



where the components T B „ e , T 3£1 (...), T Lillk , T Tool may be factorized with respect to the terms 
including the joint variables, in order to simplify computing of the derivatives (Jacobian and 
Hessian) . 

This expression includes both traditional geometric variables (passive and active joint 
coordinates) and stiffness variables (virtual joint coordinates). Explicit position and 
orientation of the end-effector can by extracted from the matrix T in a standard way 
(Angeles, 2007) , so finally the kinematic model can be rewritten as the vector function 

f = *(q,0) (2) 

where the vector t = (p, <p) r includes the position p = (x, y, z) T and orientation 
<p = (<p v , q> , cp„) r of the end-platform, the vector q = (q v q 2 , ..., q i: ) T contains all passive joint 
coordinates, the vector 8 = (0,, 6 2 , ..., Q m ) T collects all virtual joint coordinates, n is the 
number of passive joins, m is the number of virtual joints. 

2.3 Problem statement 

In general, the stiffness model describes the resistance of an elastic body or a mechanism to 
deformations caused by an external force or torque. It can be defined by the relation 
F = /(At), where /(...) is the function that associates a deformation At with an external 
force F that causes it. It worth mentioning that the function /(...) can de determined even 
for the singular configurations (or redundant kinematics) while the inverse statement is not 
generally true. For relatively small deformations, this function is defined through the 
"stiffness matrix" K , which defines the linear relation 

F = K(q ,8 )-At (3) 

between the six-dimensional translational/ rotational displacements 

At = (Ax, Ay, Az, Acp v , Acp , Atp„) r , and the static forces/ torques F =(F x , F , F, M x , M , M) 

causing this transition. Here, the vector q ={q m , q 01 , ..., q „) T includes all passive joint 
coordinates, the vector 8 =(0 O1 , O2 , ..., Om ) r collects all virtual joint coordinates, n is the 
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number of passive joins, m is the number of virtual joints. Usually, the manipulator is 
assembled without internal preloading, so the vector is equal to zero. 
However, for the loaded mode, similar relation is defined in the neighborhood of another 
static equilibrium, which corresponds to a different manipulator configuration (q, 8) , that is 
caused by external forces/ torques F . Respectively, in this case, the stiffness model 
describes the relation between the increments of the force 8F and the position 5t 

8F = K F (q,9)-8t (4) 

where q = q + Aq and 8 = 8 + A8 denote the new configuration of the manipulator, and 
Aq , A8 are the deviations of the passive joint and virtual spring coordinates respectively. 
Hence, the considered problem may be divided into three sequential subtasks: (i) finding the 
static equilibrium for the loaded configuration and checking its stability, (ii) linearization of 
the relevant force/ position relations in the neighborhood of this equilibrium, and finally (iii) 
determining the critical force for the kinematic chain that may cause undesired buckling 
phenomena. 

3. Static equilibrium for loaded mode 

Computing of the static equilibrium is a key issue for the stiffness analysis, since it defines 
the manipulator configuration (q,8) required for the linearization of the "load-deflection" 
relation. In previous works, this issue was usually ignored and the linearization was 
performed in the neighborhood of the unloaded configuration assuming that the external 
load is small enough. It is obvious that the latter essentially limits relevant results and do 
not allow to detect non-linear effects such as the buckling. From mathematical point of view, 
the problem is reduced to finding solutions of a system of non-linear equations that may be 
unique or non-unique, stable or unstable. 

3.1 Configuration of loaded manipulator 

Let us assume that, due to the external force F , the end-effector of the manipulator is 
relocated from the initial (unloaded) position t = g"(q ,8 ) to a new position t = g(q, 8), 
which satisfies the condition of the mechanical equilibrium. Here q is computed via the 
inverse kinematics and 8 is equal to zero (since there are no external loading in the 
springs), q, 8 are passive and virtual joint coordinate in the loaded mode respectively. For 
rather small displacement At = t t , a new position of the end-effector 
t = -P(q + Aq, 8 + A8) may be expressed as 

t = t +J A8 + J, Aq (5) 

where J and J are the kinematic Jacobians with respect to the coordinates 9, q, which 
may be computed from (1), (2) analytically or semi-analytically, using the factorization 
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technique. However, in general case, the model is highly non-linear and computing J and 
J requires some additional efforts. 

For computational reasons, let us consider the dual problem that deals with determining the 

external force F and the manipulator configuration (q, 8) that correspond to the output 

position t . 

Let us assume that the joints are given small, arbitrary virtual displacements 8q, 80 in the 

equilibrium neighborhood. 

According to the principle of virtual displacements, the virtual work of the external force F 

applied to the end-effector along the corresponding displacement 8t = J B S6 + J Sq is 

equal to the sum (F r J |-86 + (F r J J-8q . Since the passive joints do not produce the 

force/ torque reactions, the virtual work includes only one component -t 9 ■ 88 (the minus 
sign takes into account the force-displacement directions for the virtual spring). In the static 
equilibrium, the total virtual work of all forces is equal to zero for any virtual displacement, 
therefore the equilibrium conditions may be written as 

J/-F = t ; J/-F = (6) 

Taking into account (3), the latter system of equations can be rewritten as 

F r -J 6 = K e 6; F r -J^=0 (7) 

It is evident that there is no general method for analytical solution of this system and it is 
required to apply numerical techniques. To derive the numerical algorithm, let us linearize 
the kinematic equation in the neighborhood of the current position (q,,B,) 

t = P(q„0,) + J,(q„e,)-(q, +| -q,) + J (q„0,.)(0, +1 -e,) (8) 

and rewrite the static equilibrium equations as 

V(q„8,)-F, + , =K e, +1 ; j/(q„9,)-F, +1 =0 (9) 

This leads to a linear algebraic system of equations with respect to (q, +1 ,8 I+1 ,F j+1 ) 

J,(q,>8,)-q, +l +Jo(q,>8,)-8, +l =t-f(q„e,) + j,(q„e,)-q,+j (q„e,)-o, 
^K e -e, +1 +V(q„e,)-F +l =o (io) 

which gives the following iterative scheme 
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j e (q,.,8,)K -V(q„e,) J,(q„8,) 
J/(q,.8,) o 



t f(q„e) + J 9 (q„0,)q,+J (q„0,)0, 





(11) 



e (+l =K,- I .J, r (q i ,9 i ).F (+I 



where the starting point (q , 8 ) can be chosen using the non-loaded configuration, and 

computed via the inverse kinematics. 

As follows from computational experiments, for typical values of deformations the 
proposed iterative algorithm possesses rather good convergence (3-5 iterations are usually 
enough). However, in the case of buckling or in the area of multiple equilibriums, the 
problem of convergence becomes rather critical and highly depends on the initial guess. To 
overcome this problem, the value of the joint variables (B,,q,) computed at each iteration 

were disturbed by adding small random noise. Further enhancement of this algorithm may 
be based on the full-scale Newton-Raphson technique (i.e. linearization of the static 
equilibrium equations in addition to the kinematic one), this obviously increases 
computational expenses but potentially improves convergence. 

3.2 Stability of the static equilibrium 

To evaluate stability of the computed static equilibrium (q, 8) , let us assume that the 

manipulator end-effector is fixed at the point p corresponding to the external load F , but 
the joint coordinates are given small virtual displacements Sq , 88 satisfying the 
geometrical constraint (2), i.e. 

p=g(q,8); p = g(q + Sq,8 + S8) (12) 

For these assumptions, let us compute the total virtual work in the joints that must be 
positive for a stable equilibrium and negative for an unstable one. 

To achieve the virtual configuration (q + Sq,8 + S8) and restore the equilibrium conditions, 
each of the joints must include virtual motors that generate the generalized forces/torques 
St ? , St which satisfies the equations: 

JeF = K e 8; (J + 5J 6 ) r F = K 6 (8 + 58) + 8t 

J^F = 0; (J f +8J f ) r F = 5r ( V 

After relevant transformations, the virtual torques may be expressed as 

5t = 8(J T F)-K, 88; St, = 5(Jj F) (14) 

where S(.) denotes the differential with respect to Sq , 88 that may be expanded via the 
Hessians of the scalar function f = g(q, 8) r F : 
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8(JjF) = H£Sq + H£,8e ; 

provided that 

H^ = 8 lx V Ida 2 ; 



S(J'F)=H'Sq + H'8e 



H 



8 lx ¥ i ee 2 



H' - — u' - 
,0 - H o, 



--d 2x ¥/dqd& 



(15) 



(16) 



Further, taking into account that the virtual displacement from (q, 8) to (q + 8q,0 + 88) 
leads to a gradual change of the virtual torques from (0, 0) to (St , 8t 8 ) , the virtual work 
may be computed as a half of the corresponding scalar products 



S^ = --(S-r e '88 + ST^Sq), 



(17) 



where the minus sign takes into account the adopted conventions for the positive directions 
of the forces and displacements. Hence, after appropriate substitutions and transforming to 
the matrix form, the desired stability condition may be written as 



8W= — [S8 r 8q r 



H 6e -K„ H 



Id 



>0 



(18) 



where 8q and 86 must satisfy to the geometrical constraints (12). 

In order to take into account the relation between 8q and 88 that is imposed by (12), let us 

apply the first-order expansion of the function g(8, q) that yields the following linear 

relation 



[ J o J,] 



= 0. 



(19) 



Then, applying the SVD- factorization (Strang, 1998) of the integrated Jacobian 



[J, J,] = [U 8 D f ] 







(20) 



and extracting from V e , V the sub-matrices V e ° , V° corresponding to the zero singular 
values, a relevant null-space of the system (19) may be presented as 



89 = V ° ■ 8n; 



s q = v;-8n 



(21) 



where S\i is the arbitrary vector of the appropriate dimension (equal to the rank-deficiency 
of the Integrated Jacobian). Hence, the stability condition (18) may be rewritten as inequality 
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2 









5fi>0 



(22) 



that must be satisfied for all non-zero 8\i . In other words, the considered static equilibrium 
(q, 8) is stable if (and only if) the matrix 



H 66 -K„ 

6 ? 



K 

H F 



<0 



(23) 



is positive-negative. It is worth mentioning that the obtained result is in a good agreement 
with previous studies (Alici & Shirinzadeh, 2005), where (for manipulators without passive 
joints) the stiffness properties were defined by the matrix K e -H F that must be positive- 
definite. 

4. Stiffness model for the loaded mode 

The previous section presents a technique that allows obtaining an exact relation between 
the elastic deformations and corresponding external force/ torque. It is based on sequential 
computations of loaded equilibriums (and relevant force/ torque) for various displacements 
of the manipulator end-point with respect to its unloaded location. However, in general 
case, this relation is highly non-linear while common engineering practice operates with the 
stiffness matrix derived via the linearization. 

To compute the desired stiffness matrix, let us consider the neighborhood of the loaded 
configuration and assume that the external force and the end-effector location are 
incremented by some small values 8F , 8t . Besides, let us assume that a new configuration 
also satisfies the equilibrium conditions. Hence, it is necessary to consider simultaneously 
two equilibriums corresponding to the manipulator state variables (F,q,6,t) and 
(F + 8F,q + Sq,8 + 88, t + 8t) . Relevant equations of statics may be written as 



F ■ J — K„ -8; 



FJ, 



= 



(24) 



and 



(F + 5F) ■ (J, + SJ f = K -(8 + 58); 
(F + 5F)-(j q +5J q ) r =0 



(25) 



where 8J (q,8) and 5J (q, 8) are the differentials of the Jacobians due to changes in (q, 8) . 
Besides, in the neighborhood of (q, 8) , the kinematic equation may be also presented in the 
linearized form: 
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8t = J (q, 0) 88 + J (q, 0) 8q , 



(26) 



Hence, after neglecting the high-order small terms and expending the differentials via the 
Hessians of the function *F=g(q,8) 7 'F (similar to sub-section 3.2), equations (24), (25) may 
be rewritten as 



j/(q,e)-5F + H;(q,e)-8q + H^(q,e)-59 = K 6 -5e 
j/(q,9)-5F + H;(q,0)-5q + H; (q,0)-5e = 



(27) 



and the general relation between the increments 8F , 8t , 88 , Sq can be presented as 
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(28) 



The latter gives a straightforward numerical technique for computing of the desired stiffness 
matrix: direct inversion of the matrix in the left-hand side of (28) and extracting from it the 
upper-left sub-matrix of size 6x6. Similarly, there can be computed the matrices defining 
linear relations between the end-effector increment St and the increments of the joint 
variables 86, Sq,i.e.: 



8F = K r -St; 



Se = K„-8t; 



8q = K -St 



(29) 



where 






J , 


j 


K 


H 00 ~ K o 


H 


n 


h:„ 


H 



K„ ' K ! K„ 



(30) 



In the case when the above matrix inverse is computationally hard, the variable 86 can be 



eliminated analytically, using corresponding static equation: 80 = 



■SF + kjHj, -Sq, 



where k e =lK e — H ee ) . This leads to a reduced system of matrix equations with 
unknowns 8F and 8q 



I k F T T 

j » 9 j 

+ "ql> '*o '"e 



H + H a • k ft ■ H n 

1717 (/6 Ui/ 



8F 

8q 



(31) 



that may be treated in the similar way, i.e. the desired stiffness matrix is also obtained by 
direct inversion of the matrix in the left-hand side of (31) and extracting from it the upper- 
left sub-matrix of size 6x6: 
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(32) 



It worth mentioning that the structure of the latter matrix is similar to one obtained for the 
unloaded manipulator in (Pashkevich et al., 2009 c) and differs only by Hessians that take 
into account influence of the external load. It should be also noted that, because of presence 
of the passive joints, the stiffness matrix of a separate serial kinematic chain is always 
singular, but aggregation of all the manipulator chains of a parallel manipulator produce a 
non-singular stiffness matrix. 

Hence, the presented technique allows computing the stiffness matrix in the presence of the 
external load and to generalize previous results both for serial kinematic chains and for 
parallel manipulators. It the following Section, it will be applied to several examples that 
deal with kinematic chains employed in typical parallel manipulators. 

5. Illustrative examples 

Let us apply the developed technique to the stiffness analysis of a serial kinematic chain 
consisting of three similar links separated by two similar rotating actuated joints. It is 
assumed that the chain is a part of a parallel manipulator and it is connected to the robot 
base via a universal passive joint and the end-platform connection is achieved via a 
spherical passive joint. In order to investigate possible non-linear effects in the stiffness 
behavior of such architecture, let us consider several cases that differ in stiffness models of 
the links and actuated joints. 

5.1 Examined models 

5.1.1 Manipulator geometry 

In general, the geometry of the examined kinematic chain (Fig. 2) can be defined as UpR„R„S p 
where R, U and S denote respectively the rotational, universal and spherical joints, and the 
subscripts 'p' and V refer to passive and active joints respectively. Using the homogenous 
matrix transformations, the chain geometry may be described by the equation 



T = R„(q )T,(I)-T,(e i )-R ; (g„ 1 )-T v (I)-T l (e 2 )R z (g„ 2 )-T v (Z)-T s (e 3 )R,(q,) 



(33) 



where R_(...) and T r (...) are the elementary rotation/ translation matrices around/along 
the z- and x-axes, R„(...) is the homogeneous rotation matrix of the universal joint 
(incorporating two elementary rotations), R s (.) is the homogeneous rotation matrix of the 
universal joint (incorporating three elementary rotations), q al , q a2 are the coordinates of the 
actuated joints, L is the length of the links, q is the coordinate vector of the universal 
passive joint located at the robot base, q, is the coordinate vector corresponding to the 
passive spherical joint at the end-platform, T(.) is the homogenous vector-function 
describing elastic deformations in the links and actuators (they are represented by the 
virtual coordinates incorporated in the vectors 8,, 8 2 , 9 3 ). It is obvious that this model can 
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be easily transformed into the form 
technique. 

a) S-configuration 



t = g(q, 8) used in the frame of the developed 




Z-configuration 




b) IT-configuration c) 

A 

Fig. 3. Examined kinematical chain and its typical configurations ( Up - passive universal 
joint, Ral, Ra2 - actuated rotating joints, Sp - passive spherical joint) 

To investigate particularities of this architecture, let us also define three typical postures that 
differ in values of the actuated coordinates: 

S-configuration: the links are located along the straight line (Fig. 2a), 
the actuated coordinates are q al = q , = 

IT-configuration: the chain takes a trapezoid shape (Fig. 2b), 
the actuated coordinates are q al = q 2 = -30° 
Z-configuration: the chain takes a zig-zag shape (Fig. 2c), 
the actuated coordinates are q a] = —q , = 30° 

For presentational convenience, let us also assume that the coordinates q of the universal 

passive joint are computed to ensure location of the end-effector on the Cartesian axis x. 
For each of these configurations, let us consider three types of the virtual springs 
corresponding to different physical assumptions concerning the stiffness properties of the 
actuators/links. They cover the cases, in which the main flexibility is caused by the torsion 
in the actuators, by the link bending, and by the combination of elementary deformations of 
the links. 



5.1 .2 Case of 1 D-springs: Model A 

Here, it is assumed that the flexible elements are localized in the actuating drives while the 
links are considered as strictly rigid. It allows, without loss of generality, to reduce the 
original U p R„R„Sp model down to R p R a R a Rp and define a single stiffness parameter K B 

(similar for both actuators) that will be used as a reference value for the further analysis. 
Besides, it is possible to ignore the end-effector orientation and consider a single passive 
joint coordinate q (at the base) and two virtual joint coordinates 9,, 9 2 (at actuators). This 

restricts the end-effector motions to Cartesian xi/-plane where the geometrical model is 
defined by equations 



x = L cosq + L cosq l2 + L -cosg l3 , 
y =L sinq + L sing l2 + L -sing,, 



(34) 
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where q r = q + 9, and q n = q + 9, + 9 2 . In this case, the Jacobian matrices are also computed 
easily 



-sing -sing 12 -sing r 
cosg + cosg,, +cosg l3 



-sing 12 -sing l: 
cosg, 2 + cos g l3 



-sing, 
cosg 13 



(35) 



and corresponding stiffness analysis will be performed analytically and compared with 
numerical results that were obtained using the developed methodology. 

5.1.3 Case of 2D springs: Model B 

For this model, let us assume that the actuators do not include flexible components but the 
manipulator links are subject to non-negligible deformations in Cartesian xy-plane (bending 
and compression). Correspondingly, the link flexibility is defined by a 3x3 matrix that 
includes elements describing deformation in x- and y- directions and rotational deformation 
with respect to z-axis. Relevant stiffness matrix may be written as (Connor, 1976) 



K 



E 



A Is 

12/ -6IL 

-6-I-L 4-I-L 1 



(36) 



where L is the length of the links, / and A are respectively its second moment and area of 
the cross-section , and E is the Young module. Further, for comparison purposes, let us re- 
parameterize this matrix K to be closer to model A. In particular, let us denote the element 
kj 3 (corresponding to z-rotation) of the compliant matrix k = K" as 1 / K and eliminate 
the Young module. This yields expression 



1 



II A 







1} I 3 LI 2 
LI2 1 



(37) 



where, for a rectangular cross-section axb , the required parameters may be computed as 
A = ab and I = ab l l\2 . 

From kinematical point of view, model B is also restricted to Cartesian xi/-plane and is 
described by the expression RpR a R a Rp However, in addition to a single passive joint 
coordinate q , here there are nine coordinates of the virtual spring (three for each link). The 
kinematic model of this manipulator is defined by equations 



= i, -cosg + L 2 cosq l2 +8 2 sing,, +i 3 -cosq l} + 9 5 -sinq^ +L A -cosg l4 



■sing 14 , 



y =L X -sing +L 2 sing,. 



■cosq r + L, sinq u + 9 5 -cosq u +L 4 sinq l4 +6 S -cosq u 



(38) 
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where L X =L, L 2 =L + Q t , L 3 =L + Q 4 , L 4 =Q 7 , q l2 =q + Q,, q l ,=q + Q i +Q 6 , 
q l4 =q + Q 3 + e 6 +6 9/ and 0, = (G„ 6,, 9 3 ), 9 2 =(9 4> 6 5> 6 6 ),e 3 =(9 7> 6 S> %) are the spring joint 
coordinates for the first, second and third links respectively. The Jacobian matrices in this 
case can be also computed analytically but their dimensions are too high for analytical 
computations. Hence, in this case this stiffness analysis will be performed numerically. 

5.1.4 Case of 3D springs: Model C 

This case also assumes that that the actuators are strictly rigid but the link flexibility is 
described by a full-scale 3D model that incorporates all deflections along and around x-,y-,z- 
axes of the three-dimensional Cartesian space. Relevant 6x6 stiffness matrix of the link may 
be expresses as (Connor, 1976) 



K 
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(39) 



where A, I ,1, are the area and the second moments of the link cross-section, J is the 

polar moment, E and G are the Young Coulomb modules of the link material. For a 
rectangular cross-section axb , the required parameters may be computed as A = ab and 
I y =abl\2, L =(£• l\2 . 

Similar to previous subsection, let apply the re-parameterization by defining the compliance 
with respect the z-axis as l/K e (here, it is element k 66 of the compliant matrix k = K~ ). 
This leads to expression 



(40) 



where the coefficient k } depends on cross-section shape, k, = I 1 1_ , and v is the Poisson 

ratio coefficient. 

The kinematics of model C corresponds to the general expression U p R„R„S p (see sub-section 
5.1.1), it is described by the complete product of homogeneous matrices (33) that includes 
two passive joints (q, q, ) incorporating five passive coordinates and three virtual-springs 

with 18 virtual coordinates totally (six for each link). It is obvious that analytical 
computation in this case is rather cumbrous, so the stiffness analysis will be performed 
numerically. 
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5.2 Stiffness analysis for model A 

Let us examine first the model A that includes minimum number of flexible elements (two 
ID virtual springs in the actuated joints) and may be tackled analytically. However, in spite 
of its simplicity, this model is potentially capable to detect the buckling phenomena at least 
if the initial posture of the kinematic chain is straight (S-configuration), because of evident 
mechanical analogy to straight columns behavior under axial compression. It is matter of 
research interest to evaluate other types of initial configurations with respect to the multiple 
loaded equilibriums, their stability and to compare with numerical results provided by the 
developed technique. 

5.2.1 Computing static equilibriums 

As follows from the kinematic equations (see subsection 5.1.2), model A includes there joint 
variables ( q , 6, , 9, ) one of which may be treated as a kinematically redundant one. Let 
us assume that the redundant variable is the passive joint coordinate q while the 
manipulator end-effector is located at the point (x,y) = (3L-8, 0), where 8 is a linear 
displacement along x-axis. Then, assuming that the initial values of the actuating 
coordinates (i.e. before the loading) are denotes as 8° , 9° , the potential energy stored in the 
virtual springs may be expressed as the following function of the redundant variable 

E(q) = ^K (Q l {q)-Q° l ) 2 +^K (Q 2 (q)-Qlf (41) 



where the 9, , 9 2 are computed via the inverse kinematics as 

/ 
Q 2 (q) = +arccos 



r (3-A) 2 -2(3-A)cosg-l A 



A = 5/Z 



n , x J -sing 1 A 2sin9, 

9, (q) = atan2 - atan2 - 



[3,-A-cosqJ I (3-A) -2(3-A)cos? + l 



(42) 



Using these equations, the desired equilibriums may be computed from the extrema of 
E(q) . In particular, stable equilibriums correspond to minima of this function, and unstable 
ones correspond to maxima: 

dE(q)l dq = 0; dE (q)/dq >0 : stable equilibrium (E m ^ n ) 

dE(q)/dq = 0; dE {q)ldq <0 : unstable equilibrium ( E mnx ) 
To illustrate this approach, Fig. 4 and Table 1 present a case study corresponding to the 
initial S-configuration of the examined kinematic chain (i.e. when 9° = 9° = ). They allow 

comparing 12 different shapes of the deformated chain and selecting the best and the worst 
case with respect to the energy. As follows from these results, here there are two 
symmetrical maxima and two minima, i.e. two stable and two unstable equilibriums. 
Besides, the stable equilibriums correspond to Il-shaped deformated postures, and the 
unstable ones correspond to Z-shaped postures, as it is shown in Fig. 5. More detailed 
analysis allows deriving analytical expressions for the force and energy for small values of 8 
that will be used in the following subsection: 
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stable equilibrium: £ min » 8 ■ K e I L ; F S &K§IL 

unstable equilibrium: E max ~ 5-3Kq/L ; F s &3Kq/L 

It worth also mentioning that only stable equilibriums may be observed in practice and only 
this type of solutions is produced by the algorithm proposed in Section 3. 
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Table 1. Selected postures of the deformated kinematic chain and their corresponding 
equilibriums (case of unloaded S-configuration, 8 = L 1 10 ) 
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Fig. 4. Potential energy E(q) and manipulator postures for different values of passive 
coordinate q (case of unloaded S-configuration, 8 = L I '10) 
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Fig. 5. Evolution of the S-configuration under external loading 



5.2.2 Buckling behavior of S-configuration 

Let us apply the above results to detailed analysis of S-configuration under external loading 
in the axial direction. As follows from the previous subsection, the external force F <K e I L 
can not change the manipulator shape, similar to small compressing of straight columns that 
can not cause lateral deflections. Hence, in this case the straight configuration is stable. 
Further, for K 1 L <F < 3K g I L , the straight configuration may be hypothetically restored 
but becomes unstable, so any small disturbance will case sudden reshaping in the direction 
of a stable trapezoid-type posture. And finally, for F > 3K Q I L , there may exist two types of 
unstable equilibriums: the trivial straight-type and a more complicated zig-zag one. Hence, 
S-configuration demonstrates classical buckling phenomena that must be taken into account 
in the manipulator stiffness analysis. 

If the assumption concerning small values of 8 is released, analytical solutions for the non- 
trivial equilibriums may be still derived from the static equations. In particular, for the 
stable equilibrium, one can get 



*i(A) = 



K e tp 



(43) 



L sincp 
where cp = + arccos(l - A / 2) . For the unstable equilibrium similar equation may be written 
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^(A): 



K cos(g + 9) + 2-cosg 
L sin 9 



(44) 



:arccos 1 — 



3A A 2 



fl2-6A + A 2 

where a = ±arccos 

^ 12-4A 

Corresponding plots are presented in Fig. 6 and 7 where there are also defined the 

bifurcation points, linear approximations of the force-deflection relations and relationship 

between external force and virtual joint coordinates. Their interpretation is similar to the 

axial compression of a straight column, which is a classical example in the strength of 

materials (Alfutov, 2000). It should be noted, that the developed numerical algorithm 

exactly produces the curve (11), including "Bifurcation point 1" which defines a critical force 

that can not be exceeded in practice. For practical application, it be useful linear 

approximation at the neighborhood of this bifurcation that yields the stiffness coefficient 

0.17KJL 2 . 

Therefore, for the S-configuration, the proposed technique is able to detect and evaluate 

numerically the buckling, and it provides good agreement with engineering intuition and 

relevant mechanical analogy (compressing of the straight column). 
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Fig. 6. Model A: Force-deflection relations for S-configuration (initial unloaded posture with 
coordinates 9° = 9° = ) 




Fig. 7. Model A: Relationship between external force and virtual joint coordinates (case of S- 
configuration) 
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Fig. 8. Model A: Potential energy curves E{q) and force-deflection relations F(A) for 
selected non-straight postures 
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5.2.3 Nonlinear phenomena for other configurations 

Let us investigate now another unloaded shapes corresponding to II-configuration, Z- 
configuration and several intermediate cases. Corresponding results are presented in Fig. 8 
that contains the potential energy curves E(q) for the end-point deflection 8 = L 1 10 and 
relevant force-deflection relations F(A) . As follows from them, in most of the cases there 
exist a single stable and a single unstable equilibrium, so the kinematic chain can not 
suddenly change its shape due to external loading. The only exception is the case of II- 
configuration (see Fig. 8,- b, h) where there are two stable and two unstable equilibriums. 
Another conclusion concerns the profile of the force-deflection plots that are highly 
nonlinear in all cases. Moreover, for Z-configuration, there exists a bifurcation of the stable 
equilibriums corresponding to the cuspidal point of the function F(A) where the stiffness 
reduces sharply. 

More detailed analysis shows that Il-configuration demonstrates good analogy with axially 
compressed imperfect column where the deflection starts from the beginning of the loading 
and there is no sudden buckling, but the stiffness essentially reduces while the loading 
increases. Relevant plots are presented in Fig. 9 where the stiffness coefficient is about 
1 .78 K 1 Is at the beginning and 0.43 K B I L 2 at the end of the curve F(A) . 
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Fig. 9. Model A: Force-deflection relations and deformations in actuated joints for II- 
configuration (initial unloaded posture with coordinates 0° = 0° = -30° ) 



However, for Z-configuration that corresponds to the unloaded zig-zag shape, the stiffness 
behavior demonstrates the buckling that leads to sudden transformation from a symmetrical 
to a non-symmetrical posture as shown in Fig. 10. Here, there exist two stable equilibriums 
that differ in the values of the potential energy (see Fig. 8 e, k). Relevant plots are presented 
in Fig. 11 where the stiffness coefficient is about 16.7 K / L 2 at the beginning and 

0.39 K e I L 2 at the end of the curve F(A) . 
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Fig. 10. Evolution of the Z-configuration under external loading 
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Fig. 11. Model A: Force-deflection relations and deformations in actuated joints for Z- 
configuration (initial unloaded posture with coordinates 6° = -30°; 6° = 30° ) 



Therefore, the stiffness analysis of model A (Table 2) allowed detecting more general class of 
manipulator postures that are dangerous with respect to the buckling. They include all 
configurations that posses an axial symmetry with respect to the direction of the external 
force (S- and Z-configurations for instance). These postures will be in the focus of the 
stiffness analysis for models B and C. 
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Table 2. Summary of the Stiffness analysis for model A 
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5.3 Stiffness analysis for model B 

In this case, it is assumed that the manipulator stiffness is caused by elasticity of the links 
while the actuating joints are rigid enough. The elastic deflections (bending and 
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compression) are still restricted by the Cartesian xy-plane and each link includes only three 
virtual springs with joint variables 9' v , 9 and Q'^_ , which describe respectively linear 

displacements in x- and y-directions and angular rotation around z-axis. Totally, the 
stiffness model has 11 variables (two for a passive joint and nine for the virtual springs of 
three links), so it was studied numerically, using the proposed technique. The stiffness 
parameters were evaluated assuming that the links are rectangular beams of the length L 
and the cross-section axb, where a = 0.02L and b = 0.05L . For comparison purposes, 
corresponding stiffness matrices were scaled with respect to the bending coefficient to keep 
similarity with model A (see sub-section 5.1.3). The stiffness analysis was performed for 
three above mentioned typical configurations, assuming that the external force is directed 
along the x-axis causing compression of the examined kinematic chain. 

For S-configuration, the results are presented in Fig. 12 that includes both the force- 
deflection plot and plots for deflections in the virtual springs. As follows from these results, 
here also there is very strong analogy with the compression of the straight column. In 
particular, first the links are subject the compression and the deflection starts from the 
beginning of the loading but the stiffness is very high (about 2500 K B I Is , for the assumed 
link shape). Then, after the buckling, the kinematic chain changes its shape to become non- 
symmetrical and the stiffness falls down to 0.20 K e /L 2 . The critical force may be also 
computed using the previous results, as F Q = K I L . 

For ri-configuration (Fig. 13), the stiffness properties are also qualitatively equivalent to the 
case of model A but the stiffness coefficient is slightly lower (in the frame of the adopted 
parameterization). For the presented curve F(A) , it varies from 5.31 K lis to 0.34 K II 2 . 
For Z-configuration (Fug. 14), it has been also detected the buckling that occurs if the 
loading approaches to the critical value F = 1 .07 K a 1 1 . At this point, the stiffness falls 

down from 100 -K a I Is to 0.13 K 1 Is , which essentially differs from model A due to 
different nature of the virtual springs and to the cross-coupling between them. Here, it 
should be taken into account that the adopted parameterization ensure equivalence of the 
rotational compliance \/K e in virtual springs of models A and B, but their rotational 

stiffness is different. 

Hence, the obtained results (Table 3) demonstrate qualitative similarity but some 
quantitative difference compared to model A. The latter is caused by different arrangement 
of the elastic elements in the virtual joints that corresponds to other physical assumptions. 
These results confirm essential influence of the external loading on the manipulators 
stiffness and potential instability of symmetrical postures. 



354 



Advances in Robot Manipulators 



F-L 



*r 






\J 
















>*C/6ivergence of the 
S equilibrium 







2. A 6 
x10'* 



J 

0.15 

0.1 




s = 


i — y 




3 = L!1 / 1 








0.05 


Diverge 
equ 


nee of the 


ibrium \ 







sxh-io^L \ 







9 * 
0.3 

0.2 

0.1 





Divergence of the 
equilibrium 



SIL 



1 £■£ 
K 



1 £v£ 
A.' 



(c) 



(a) (b) 

Fig. 12. Model B: Force-deflection relations and deflections in virtual springs for S- 

configuration (initial unloaded posture with coordinates 9° = Q° 2 = ) 
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Fig. 13. Model B: Force-deflection relations and deflections in virtual springs for II- 
configuration (initial unloaded posture with coordinates 9° = Q° 2 = -30° ) 
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Fig. 14. Model B: Force-deflection relations and deflections in virtual springs for Z- 

configuration (initial unloaded posture with coordinates 9° = -30°; 9° = 30° ) 
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Configuration 
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Table 3. Summary of the Stiffness analysis for model B 
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5.4 Stiffness analyses for the model C 

Finally, let us consider model C where the link elasticity is described in 3D space and 
corresponding stiffness matrices have dimension 6x6 (the actuating joints are assumed 
perfect and rigid, similar to model B). It is also assumed that the links are rectangular 
beams of the length L with the cross-section axb, where a = 0.02L , b = 0.05L and the 
smaller value a corresponds to z-direction that was not studied above. The latter 
assumption agrees with real dimensions of links used in some parallel manipulators, such as 
Orthoglide (Chablat & Wenger, 2003). 

To ensure comparability of all examined cases, the link stiffness matrices were 
parameterized with respect to the bending coefficient of the z-axis K (see sub-section 5.1.4). 
In total, the stiffness model includes 23 variables (five for passive joints and 18 for the 
virtual springs of three links) and it was studied numerically. The stiffness analysis was 
performed for the same manipulator configurations (S, IT and Z) in the unloaded mode and 
the same direction of the external force as for models A and B. 

For S-configuration, the results (Fig. 15) are qualitatively similar to ones obtained for model 
B. Besides, numerical value of the stiffness for the non-loaded case is the same, 2500 K / L 2 . 
However, here the buckling occurs for essentially lower critical force, 0.16 K s /L, that 
corresponds to sudden lateral deflection in z-direction. Then, after the buckling, the stiffness 
falls down to 0.20 K 1 Is . It worth mentioning that the axial deflection corresponding to the 

critical force is very low, it is equal to 7 >10" • 8 / L . But further increase of the force by only 
20% leads to extremely high increase of the deflection, in more then 1000 times. 
In contrast, for IT-configuration (Fig. 16), it was detected buckling that does not exist in 
models A and B. In particular, if the external force exceeds the critical value 0.20 K 1 L the 

stiffness suddenly reduces from 1.03 K 6 /L 2 to 0.04 K e / L 1 (for comparison, the stiffness 

coefficient for unloaded mode is 1.70 K 11} ). Physically it is also explained by sudden 

deflection in z-direction that it was beyond capabilities of previous models. It worth also 
mentioned that, in this case study, the stiffness of manipulator links in z-direction is 
essentially lower than in y-direction. Another interpretation of this buckling phenomena 
may be presented as sudden loss of symmetry with respect to xy-plane. 
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For Z-configuration (Fig. 17), the results remain qualitatively the same, but corresponding 
numerical values are changed. Thus, manipulator stiffness for the unloaded mode is 
7.52 K 1 L , it gradually reduces to 5.88 K I L and then, after the buckling, falls down to 
0.03 K a l L . Corresponding value of the critical force is 0.17 K B I L and is also defined by the 

z-direction deflection. 

More detailed numerical results concerning model C are presented in Table 4. As follows 
from them, a full-scale 3D stiffness analysis yields lower values of critical force compared to 
models A and B. Besides, for model C, all examined postures demonstrated buckling related 
to sudden deflections in the z- direction. This presents another source of potential structural 
instability of kinematic chains that posses the symmetry with respect to a plane. 
Generally, summarizing all presented case studies, it should be concluded that the 
developed technique produces reliable results, it is able to evaluate manipulator stiffness 
and to compute the range of the loading that prevents buckling. 



0.8 

0.6 

0.4 

0.2 






0.4 

0.3- 
0.2- 
0.1 



Divergence of the 
equilibrium 
t>'«7KJ'Z 






s 


' 1 








1 








I 




divergence of the 
equilibrium 


1 




dollar- L 


\\ 





SIL 



0.15f.i 
K 



(a) 



(b) 



0.1 0.15/7./, 

K 
(C) 



Fig. 15. Model C: Force-deflection relations and deflections in virtual springs for S- 
configuration (initial unloaded posture with coordinates 6° = 9° = ) 



F-L 
K f 

0.2 

0.15 
0.1 

0.05 






/\d2S 












/ D-.1S 


















1 o.o: 


y^Divergen 


ce of the 


/ 


/ 




/ 


□.05 : 0.1 0.15 0.2 



0.3 
0.2 

0.1 









i>-0.9.i — -i 




























Divergence of the 
equilibrium \ 






£«0.17-£ I \ 







0.6 
0.4 
0.2 





Divergence of the 
equilibrium 




SIL 



0.05 0.1 0.15 0.2 F-L 
K 
(a) (b) 

Fig. 16. Model C: Force-deflection relations and deflections in virtual springs for H 
configuration (initial unloaded posture with coordinates 6° = 0° = -30° ) 



0.05 0.1 0.15 0.2 p. I 
K 
(C) 



Enhanced stiffness modeling of serial manipulators with passive joints 



357 




0.4 
0.3 
0.2 

0.1 



S = L 1 


l 


S 


= j\/2—J 




Divergence of the 
equilibrium 
d* 0,028-1 


_\J 





ry 

0.8 

0.6 

0.4 

0.2 





Divergence of the 
equilibrium 
5*0.038-/ 




SIL 



(a) 



0.05 0.1 0.15 F-L 

K 

(b) 



0.05 0.1 0.15 F-L 

K 
(C) 



Fig. 17. Model C: Force-deflection relations and deflections in virtual springs for Z- 
configuration (initial unloaded posture with coordinates 6° = -30°; 0° = 30° ) 
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Fig. 18. Model C: Evolution of the IT- and Z-configurations under external loading 
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Table 4. Summary of the Stiffness analysis for model C 



6. Conclusions 

In modern robot-based manufacturing, the stiffness analysis becomes a critical issue. It is 
motivated by current trends in manipulator design that are targeted at achieving high 
dynamic performances with relatively small link masses and low energy consumption in 
actuators. These demand a revision of the existing stiffness modeling techniques that 
must take into account the external loading imposed by a manufacturing process. 
Accordingly, this chapter focuses on the enhanced stiffness modeling and analysis of 
serial kinematic chains with passive joints, which are widely used in parallel robotic 
systems. In contrast to previous works, the stiffness is evaluated for the loaded working 
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mode corresponding to the static equilibrium of the elastic forces and the external wrench 
acting upon the manipulator end point. The proposed technique allows finding the full- 
scale "load-deflection" relation for any given workspace point and to linearise it taking 
into account variation of the manipulator Jacobian due to the external force/ torque. These 
enables designer to evaluate critical forces that may provoke non-linear behavior of the 
manipulators, such as sudden failure due to elastic instability (buckling) which has not 
been previously studied in robotic literature. 

One of the essential novelties proposed here is a new solution strategy of the kinetostatic 
equations, which takes into account the passive joints in the straightforward way, 
allowing computing a stiffness matrix even for singular Jacobian and Hessian. Besides, 
the method does not require manual model reduction that usually deals with elimination 
of the redundant springs corresponding to the passive joints, since this operation is 
inherently included in the numerical algorithm. Another advantage is the computational 
simplicity that requires low-dimensional matrix inversion compared to other techniques. 
The theoretical contributions also include the matrix criteria for the stability of the static 
equilibrium in the loaded mode. 

The advantages of the developed technique are illustrated by several examples that deal 
with kinematic chains employed in typical parallel manipulators. They demonstrate 
possible non-linear effects that may arise in loaded mode, including essential dependence 
of the stiffness on the applied force/ torque and sudden change of the stiffness if the 
external wrench exceeds the critical value. Besides, there were detected several typical 
configurations of serial kinematic chains that are potentially dangerous with respect to 
buckling. It is shown that such configurations possess either axial or planar symmetry 
with respect to direction of the external loading. This research may be also extended for 
more sophisticated architectures that include parallel manipulators with intermediate 
links between the main kinematic chains, kinematic parallelograms and other structures 
improving rigidity of the manipulating system. 
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1. Introduction 

Dynamic models play an important role in parallel manipulators simulation and control. 
Mainly in the later case, the efficiency of the involved computations is of paramount 
importance, because manipulator real-time control is usually necessary (Zhao & Gao, 2009). 
The dynamic model of a parallel manipulator operating in free space can be represented in 
Cartesian coordinates by a system of nonlinear differential equations, which may be written 
in matrix form as 

l(x)-x + V(x,x)-x + G(x) = f (1) 

l(x)being the inertia matrix, V(x,x) the Coriolis and centripetal terms matrix, G(x) a vector 

of gravitational generalized forces, x the generalized position of the moving platform (or 
end-effector) and f the controlled generalized force applied on the end-effector. Thus, 

f=J r (x)-T (2) 

where t is the generalized force developed by the actuators and J(x) is the inverse 
kinematics jacobian matrix (Merlet, 2006). 

Dynamic modelling of parallel manipulators presents an inherent complexity, mainly due to 
system closed-loop structure and kinematic constraints. Several approaches have been 
applied to the dynamic analysis of parallel manipulators, the Newton-Euler and the 
Lagrange methods being the most popular ones (Do & Yang, 1988; Reboulet & Berthomieu, 
1991; Ji, 1994; Dasgupta & Mruthyunjaya, 1998; Khalil & Ibrahim, 2007; Riebe & Ulbrich, 
2003; Guo & Li, 2006; Nguyen & Pooran, 1989; Lebret et al., 1993; Di Gregorio & Parenti- 
Castelli, 2004; Caccavale et al., 2003; Dasgupta & Choudhury, 1999). These methods use 
classical mechanics principles, as is the case for all the approaches found in the literature, 
namely the ones based on the principle of virtual work (Staicu et al., 2007; Tsai, 2000; Wang 
& Gosselin, 1998), screw theory (Gallardo et al., 2003), recursive matrix method (Staicu & 
Zhang, 2008), Hamilton's principle (Miller, 2004), and Kane's equation (Liu et al, 2000). 
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Thus, all approaches are equivalent, leading to equivalent dynamic equations. Nevertheless, 
these equations can present different levels of complexity and associated computational 
loads (Zhao & Gao, 2009). Minimizing the number of operations involved in the 
computation of the manipulator dynamic model has been the main goal of recent proposed 
techniques (Zhao & Gao, 2009; Staicu & Zhang, 2008; Abdellatif & Heimann, 2009; Wang et 
al, 2007; Sokolov & Xirouchakis, 2007; Bhattacharya et al, 1997; Carricato & Gosselin, 2009; 
Lopes, 2009). 

This book chapter presents the generalized momentum concept to model the dynamics of a 
Stewart platform manipulator having a non-stationary base platform. This is important, for 
example, in macro/ micro robotic applications, where a small manipulator is attached in 
series to a big one. The later performs large amplitude movements, while the former is only 
responsible for the small motions. The book chapter is organized as follows. Section 2 
presents a brief description of the parallel manipulator under study. In section 3 a complete 
dynamic model is developed. The generalized momentum approach is used and the motion 
of the manipulator base platform is considered. Conclusions are drawn in section 4. 

2. Manipulator Kinematic Structure 

A Stewart platform manipulator is being considered. It comprises a (usually) fixed platform 
(the base) and a moving platform (the payload platform), linked together by six 
independent, identical, open kinematic chains (Raghavan, 1993). In this book chapter a 
particular design will be considered as shown in Figure 1 (Fichter, 1986). In this case, each 
chain (leg) comprises a cylinder and a piston (or spindle) that are connected together by a 
prismatic joint, /,. The upper end of each leg is connected to the moving platform by a 
spherical joint whereas the lower end is connected to the fixed base by a universal joint. 
Points Bt and P, are the connecting points to the base and moving platforms, respectively 
(Figure 2). They are located at the vertices of two semi-regular hexagons inscribed in 
circumferences of radius rs and rp. The separation angles between points B\ and Bf,, Bi and 
B3, and Bi and B5 are denoted by 2</>b. In a similar way, the separation angles between points 
Pi and P2, P3 and P4, and P5 and P(, are denoted by 2tf>p (Figure 2). 



Moving platform 




Fig. 1. Stewart platform kinematic structure 
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B s p, 

Fig. 2. Position of the connecting points to the base and payload platforms 

For kinematic modelling purposes, two frames, {P} and {B}, are attached to the moving and 
base platforms, respectively. Its origins are the platforms centres of mass. The generalized 
position of frame {P} relative to frame {B} may be represented by the vector: 



Vr 



<PrT =["<<, 






(3) 



where °x p 

and 8 x„,., 



Wr 



y p z p \ is the position of the origin of frame {P} relative to frame {B}, 
cp p \ defines an Euler angles system representing orientation of frame 



{P} relative to {B}. The used Euler angles system corresponds to the basic rotations 
(Vukobratovic & Kircanski, 1986): y/p about zp; 0p about the rotated axis yp-; and cpp about the 
rotated axis xp". The rotation matrix is given by: 



R„ 



Cy/ p C0 p Ci// p S0 p S(p p -Si(r p C(p p 
Sy/ p CO p Sy/ P S0 p S<p p + Cy/ p C<p p 
-S0 P C0 p Scp p 



Cy/ p S0 p C<p p +Sy/ p S<p p 
Sy/ p S0 p C<p p -Cy/ p S(p p 

C0 P Ccp p 



(4) 



S() and C() correspond to the sine and cosine functions, respectively. 

The manipulator position and velocity kinematic models are well known, being obtainable 
from the geometrical analysis of the kinematics chains. The velocity kinematics is 
represented by the Euler angles jacobian matrix, Jp, or the kinematic jacobian, Jc (Merlet, 
2006). These jacobians relate the velocities of the active joints (the actuators) to the 
generalized velocity of the moving platform: 
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l = j.»i =j . 



l = J, J x„ =J • 



with 



1 = V, I 



= J/i. 



(5) 
(6) 

(7) 
(8) 



and (Vukobratovic & Kircanski, 1986) 



-Syr, C6 p Cy/ r 

Cy, C0 p Sy/ p 

1 o -se. 



(9) 



Vectors "i . , =*v p and "co, , represent the linear and angular velocity of the moving 
platform relative to {B}, and *i, w represents the Euler angles time derivative. 



3. Complete Dynamic Modelling Using the Generalized Momentum Approach 

It is well known the generalized momentum of a rigid body, q c , may be computed from the 
following general expression: 

q,=I,u, (10) 

Vector u c represents the generalized velocity (linear and angular) of the body and I c is its 

inertia matrix. Vectors q r and u c and inertia matrix I c must be expressed in the same frame of 

reference. 

Equation (10) may also be written as 



I 







I 



(11) 



where Q c is the linear momentum vector due to rigid body translation and H c is the angular 
momentum vector due to body rotation. I c (i ra ) is the translational inertia matrix and \ c (mt) is 
the rotational inertia matrix. v c and a c are the body linear and angular velocities. 
The inertial component of the generalized force (force and moment) acting on the body can 
be obtained from the time derivative of equation (10): 
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f ( ,=q =1 u +1 li (12) 

with force and moment expressed in the same frame and f , {lm) = \F* llm) M t r ( . J . 
Equivalently, force and moment vectors are: 

F (te) = Q = I _.,„.„, ■ v, + i„ ( „, ■ v, (13) 

M iW = H . = I_. lm| -a c +i cba) <o (14) 

3.1 Payload Platform Modelling 

Considering the Stewart platform manipulator base motion, i.e., the motion of frame {B} 
relative to a fixed world frame {W}={xw, yw, zw), the position of the payload platform, {P}, 
relative to {W} and expressed in {W}, may be given by 

where " p a . is the position of frame {B}, and "p p . represents the position of frame {P} 

relative to {B} and expressed in {W}. 

The linear velocity of the payload platform, {P}, relative to {W} and expressed in {W}, may 

be obtained taking the time derivative of the previous equation, that is, 

, "v P|i „="v B| ,„ + "v f| „, + "a, fi|ir X«p f||| (16) 

where " v is the linear velocity of frame (Bl, * v is the linear velocity of frame (P! as 

be F in- 

seen by an observer fixed in {B}, " to a represents the angular velocity of frame {B} relative 

to {W}, and *p f ="x represents the position of {P} relative to {B} and expressed in {W}. 

In the following analysis, knowledge of the generalized position of frame {B} relative to {W}, 
r x„ =[x, v. z„ w n 6> 0,] T =["'x T ., ,, Y„ f , shall be assumed: 

" x represents the position vector expressed in frame {W}, and " x represents the 

orientation expressed in an Euler angles system. Knowledge of its first and second time 
derivatives shall also be supposed i.e., " x B and " x , respectively. Therefore, the 

orientation matrix, " R a , of frame {B} relative to {W} can be easily computed, and the 

jacobian, J& relating the angular velocity of the base frame relative to {W}, " to s , to the first 

time derivative of the Euler angles, " x a , is given by 

r ».u= J .' ri *)i. W 
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Considering equation (16), in frame {B}, the following equation can be written: 

'^r^i,^i, + ''».i, x 'i'N, ( 18 > 

Therefore, the total linear momentum of {P} expressed in frame {B} will be 

Q„„,U="V"v fU (19) 

rap being the pay load platform mass. 

Taking the time derivative of the previous equation results in 



Q*.„.=»/*,i. (2°) 



Knowing that, 



results in 

, ^=^l,^l, +2,ffl .|/^|+'<|/ , P r |/'» 1| /K l /X l J (23) 



The inertial part of the total force applied in {P}, due to the payload platform translation, 
expressed in frame {B} will be 

VirQ*)l (2 4 ) 

That is, 

'W,,^/^ (26) 

%M M L = «, 'K |. +2 '°* l/ V > l/'^ l/*P" l. + " '*. A'*' U X 'P> J ( 27 ) 

where T , . represents the inertial part of the force, considering the base platform is not 
moving, and F F P . . , , . represents the inertial part of the force which results from the base 
motion. 
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On the other hand, the angular momentum of the moving platform, about its centre of mass 
and expressed in frame {B} will be 

H, ( „ )U =!„,,,/ <», , (28) 

I represents the rotational inertia matrix of the moving platform, expressed in the base 

frame, {B}. This inertia matrix is given by: 

h lr „„ lB = B K-K„ >l /K (29) 

where I is a constant matrix representing the rotational inertia matrix of the moving 

platform, expressed in frame {P}. Considering that /, , /, and /,, are the moments of 
inertia of the moving platform expressed in its own frame, this matrix may be written as: 

I nmtUp =diag([/ Pii I Pr I J) (30) 

Taking the time derivative of equation (28) results in 

H , = I , , , ■"' co . + 1 , , . ■"' to . (31) 

The inertial part of the total moment applied in {P}, due to the payload platform rotation, 
expressed in frame {B} will be 



that is, 



where, 



°M 



-;.(„„■)(» 



P M, WMU =H, MU (32) 

P M, w( „, )U ='M, wwlB+ 'M p(te)( )u (33) 



'M f| . represents the inertial part of the moment, considering the base platform is not 

moving, and 'M represents the inertial part of the moment which results from the 

base motion. 

The total inertial component of the generalized force applied to {P} and expressed in {B} will 

be 



368 Advances in Robot Manipulators 

p f,M M u=rF; M( „, )u 'm;,^,,/ (36) 

The inertial components of the forces in the manipulator actuators (actuating forces) will be 

T f(,™)(./i.) = •'c ' f ,(„„)(/,,) \ B (37) 

T,(,„x„„„)= J c r/f „,„,, l „„„, u ( 38 ) 

On the other hand, regarding the gravitational part of the generalized force, if the base 
platform orientation changes, then the force applied to {P} and expressed in {B} results in 

% Mll = W K-X Mlw (39) 

where, 

o "R.J (40) 

and ''i p[ is the gravitational generalized force applied to {P} and expressed in {W}. This 

force can be computed using a simplified model that considers both a non-moving base 
platform, frame {B} parallel to {W}, and z B = -g , i.e., 






, f = •<_^ LL (41) 



P F =m p ■ g-z p representing the mobile platform potential energy. 

The gravitational component of the actuating forces due to the moving platform, t,, , , , is 

given by equation (42), which can be added to equations (37) and (38). 

^,=j;''f, Hll (42) 



3.2 Cylinder Modelling 

Position of the cylinder i, relative to {W} and expressed in {W}, may be computed using the 
following equation: 

" P, ., =" P. | +*P r . (43) 

The linear velocity of the cylinder, relative to {W} and expressed in {W}, may be obtained 
taking the time derivative of the previous equation, that is, 
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c i In- B lie i . 



<-, |„, a I,,.. t, | 



(44) 



Considering that frame {C,j is attached to the cylinder i and positioned at its centre of mass, 

then " v is the linear velocity of frame {C} as seen by an observer fixed in {B}, and *p 

c i \ w j j - <-, iii- 

represents the position of {C,j relative to {B} and expressed in {W}. 
In frame {B} the following equation can be written: 



"». U x P - 



(45) 



Considering the centre of mass of each cylinder is located at a constant distance, be, from the 
cylinder to base platform connecting point, B„ (Figure 3), then its position relative to frame 
{B} is 



where, 



X, =Vl,+b, 



1 1 



1 l 



l,= ^w, + P, -b, 



(46) 

(47) 
(48) 




Fig. 3. Position of the centre of mass of the cylinder i 

The linear velocity of the cylinder centre of mass, "p r , relative to {B} and expressed in the 
same frame, may be computed as: 

»p => xb e -l, (49) 



where "to ( represents the leg angular velocity, which can be found from: 
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"to, xl.Vv +*€» x p p 



(50) 



As the leg (both the cylinder and piston) cannot rotate along its own axis, the angular 
velocity along 1 is always zero, and vectors 1, and s co ( are always perpendicular. This 
enables equation (50) to be rewritten as: 






or, 



°,|. x P. 



"to =J„ 



* 10 



(51) 



(52) 



where jacobian J D is given by: 



1 



1 1/P, r 

1 



r i 



(53) 
(54) 



and, for a given vector a = [a v a a_\ , 






- a_ 


a 


a_ 





— a t 


-a 


a 






(55) 



On the other hand, equation (49) can be rewritten as: 



Pc, 



" CO.. 



(56) 



where the jacobian J u is given by: 



b c -V -\ b c V \ F ^ T 



(57) 



The total linear momentum of the cylinder i, expressed in frame {B} will be 



Fast Dynamic Model of a Moving-base 6-DOF Parallel Manipulator 371 

Qomu="vX Ib (58) 

mc being the cylinder mass. 

Taking the time derivative of the previous equation results in 

Q c , W u="v%i s ( 59 ) 

where, 

'** U = "' V « \. + '*« I, +r<a ' l/X \. + "'< l/X L + '°* I. X K I, X X I.) ( 6 °) 

The inertial part of the total force applied in {C,}, due to the cylinder translation, expressed 
in frame {B} will be 

C 'F, M( ,„, )|s =Q c , M|B (61) 

That is, 

r 'F = r 'F + C 'F (62) 

'qM(».)Ij ti*]|, (iM-ll, *•" ' 

C ' F c jW wu =m c'\ ,. = w c • V^u +"V ■ V^u ( 63 ) 

e % MW U = »c -K |. +2 "°>» l/X U + '< l/X ,.+"». l/K ,. *X J) (64) 

where C 'F w( . . represents the inertial part of the force, considering the base platform is 
not moving, and c 'F c represents the inertial part of the force which results from the 

base motion. 

When equation (61) is pre-multiplied by i\ , the inertial component of the generalized force 

applied to {P}, due to each cylinder translation is obtained in frame {B}: 

The inertial components of the actuating forces will be 

T c,M(7*)M = J c ' f c>.)U«)M 1, ( 66 ) 

T c,M(~»)M = 3 c ' f (,„,)(„„„i(,™) | a ( 67 ) 

On the other hand, the total angular momentum of the cylinder about its centre of mass and 
expressed in frame {B} will be: 
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H, MIj =I CiMI /<o, |b (68) 

Taking the time derivative of the previous equation results in 

H , .. =1 , ., "a . +1 , ., -"'to . (69) 

where, 

'^r , ^ ll ^. l / , «», l ^"< l) (7i) 

Considering that I r , . , is the inertia constant matrix of the rotating cylinder i, expressed in 
the frame fixed to the cylinder itself, {Q}={ x c . ,y c ,z c }, then 

^Ml^X^cMle/^c, (72) 

where "R r is the orientation matrix of each cylinder frame, {C,j, relative to the base frame, 

{B}. 

Cylinder frames were chosen in the following way: axis x c coincides with the leg axis and 

points towards P,; axis y c is perpendicular to x c and always parallel to the base plane, this 

condition being possible given the existence of a universal joint at point B„ that negates any 
rotation along its own axis; axis z r completes the referential following the right hand rule, 

and its projection along axis zg is always positive. Thus, matrix ! R C becomes: 

s R Ci =[x Cj y C( zj (73) 

where 

x c = 1, (74) 



1 



(75) 



^11 +11 ^11 +11 

z c , = x Q xy Q (76) 

So, the inertia matrices of the cylinders can be written as: 

I ClM|Q =diag([7 CD I Crr I C J) (77) 

where I c , I c and I c are the cylinder moments of inertia expressed in its own frame. 
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The inertial part of the total moment applied in {C,{, due to the cylinder rotation, expressed 
in frame {B} will be 



that is, 



where, 



C 'M C(MM| ,=H CiMU (78) 

'■M q( „„ k „, 1|b ^M q( ,„ M/HU+ -M q( „„ (m1U (79) 



XMwr^Lf'.Kt^f'v'i^ ( 8 °) 

" M,, H u = i, M u " •, u +I„ w u ' K u +'•. ,/' •„ s ) (81) 

r 'M c ,, . represents the inertial part of the moment, considering the base platform is not 
moving, and c 'M c (J ,, represents the inertial part of the moment which results from the 

base motion. 

When equation (78) is pre-multiplied by J' , the inertial component of the generalized force 

applied to {P}, due to each cylinder rotation is obtained in frame {B}: 

The inertial components of the actuating forces will be 

T c J M(M«) = •* c ' 'qMOiOM 1, ( ) 

T C,M(~»)M = J C ' f Q |„„.)(„„„)(„,) |, (° 4 ) 

Now, with reference to the gravitational part of the generalized force, if the base platform 
orientation changes, then the force applied in {P} and expressed in {B} results in 

' f * 1L =X'f« t (85) 

where T c( . is the gravitational generalized force applied in {P} and expressed in {W}. 

This force can be computed using the model that considers both a non-moving base 
platform, frame {B} parallel to {W}, and z B m — g, i.e., 

, f = — ._\ — u^ (86) 
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P being the cylinder potential energy. Using equation (46) it can be computed by: 



P^=m c -g.^(z p V Pl J (87) 



The gravitational component of the actuating forces due to each cylinder, t c , ,, , is 



T - I P f 

T c,M J c *c, Ml 



3.3. Piston Modelling 

Position of the piston i, relative to {W} and expressed in {W}, may be computed using the 
following equation: 

The linear velocity of the piston, relative to {W} and expressed in {W}, may be obtained 
taking the time derivative of the previous equation, that is, 

"v s| = r v +X +'»., X "P, ( 9 °) 

Considering that frame {S,j is attached to the piston i and positioned at its centre of mass, 
then °\ s . is the linear velocity of frame {S,j as seen by an observer fixed in {B}, and"p s 

represents the position of {S,j relative to {B} and expressed in {W}. 
In frame {B} the folloiving equation can be written: 

l, ' T ,i,- rv .i, +,v ,i +l,ffl .i, x X l , ( 91 ) 

If the centre of mass of each piston is located at a constant distance, bs, from the piston to 
moving platform connecting point, P„ (Figure 4), then its position relative to frame {B} is: 

*p . =-b -L+'p +*x , ,, (92) 

The linear velocity of the piston centre of mass, *p s , relative to {B} and expressed in the 
same frame, may be computed as: 

s p,i =».+>, i. x U-0 ( 93 > 
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J„ 



v 

p 



(94) 



where the jacobian J ( , is given by: 



J 



i-6, r \ \i-b,-\ T -\\'p: 



(95) 




Fig. 4. Position of the centre of mass of the piston i 

The total linear momentum of the piston i, expressed in frame {B} will be 



%M' ->" X 



") l» s s . 



ms being the piston mass. 

Taking the time derivative of the previous equation results in 



Q.MI =».'*. 



where, 



"▼» i ="' V .i +V S i +2 " a> i | x " v s i +r < X "P S i +" a> «i x ("' a> i | X *P, i ) 

s > \r b b s ' b s Is s ' b » » s i s B b V s s 5 ' b / 



(97) 



The inertial part of the total force applied in {S,}, due to the piston translation, expressed in 
frame {B} will be 



That is, 



s, (,.«)(»,) I, "*s,(»)| 



<F — * F + S 'F 



(99) 
(100) 
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Xmmi. =».'K i. +2 " w « i. x '\ t +r *. ia X X k + '»* I . x i ra, » i* X X J ( 102 ) 

where S; F s . . represents the inertial part of the force, considering the base platform is 
not moving, and '' F s ... , represents the inertial part of the force which results from the 

base motion. 

When equation (99) is pre-multiplied by i J a , the inertial component of the generalized force 

applied to {P}, due to each piston translation is obtained in frame {B}: 

%M<»x~>u ='»/'*;<«*,,, ( 103 ) 

The inertial components of the actuating forces will be 

T i,M(A)M =J c ' f i, („„)(».,-)(,„ )| B (104) 

T s,M(~)M = J c *' f j,(a,)(™)M |, ( 105 ) 

On the other hand, the total angular momentum of the piston about its centre of mass, 
expressed in frame {B}, will be: 

Taking the time derivative of the previous equation results in 

H , , =1 , ,, -"'to . +1 .. "to . (107) 

J|M U s '(™'> la % la s iM la s ' la V ' 

where, 

(108) 

m, , (109) 

Considering that I s , . is the inertia constant matrix of the rotating piston i, expressed in 
the frame fixed to the piston itself, {S,j, then 

where *R S is the orientation matrix of each piston frame, {S,}, relative to the base frame, {B}. 
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As the relative motion between cylinder and piston is a pure translation, {S,j can be chosen 

parallel to {C,}. Therefore, "R S/ = *R C] . 

So, the inertia matrices of the pistons can be written as: 

I SiM|j =diag([/ Jvv I Srr I S J) (111) 

where I s , I s and I s are the piston moments of inertia expressed in its own frame. 

The inertial part of the total moment applied in {S/}, due to the piston rotation, expressed in 
frame {B} will be 



that is, 



where, 



M SlMMI =H sW , (112) 



*M^, L ^M_,+«M ft( ,,. ,, (113) 



"■»,MM L SiMW lj ijM(«~) 



'M 



s, (.■..■!( A.. 



(ft 



(W'JsiKl , +I *MU- J V%t ( U4 ) 



* M,,,,,,,,,,, u = I S|M L ■"■ c B u + 1,,,.,,,, u ■ ("c, u +"'0,, L x> «o S/ u ) (115) 

1 M s . , . represents the inertial part of the moment, considering the base platform is not 
moving, and s 'M s (i i, , , represents the inertial part of the moment which results from the 

base motion. 

When equation (112) is pre-multiplied by J^ , the inertial component of the generalized 

force applied to {P}, due to each piston rotation is obtained in frame {B}: 

%MMM,,= J V' M *M M , J (116) 

The inertial components of the actuating forces will be 

T s,M(*)M = ""c ' 'sjMMM \ B (117) 

T s,(i,,)(™)M = J c ' f s ,.|i„ k™„)(„,] | a (US) 

Now, with reference to the gravitational part of the generalized force, if the base platform 
orientation changes, then the force applied in {P} and expressed in {B} results in 

% MlB = w K-% Mlw (H9) 
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where 'f J( , is the gravitational generalized force applied in {P} and expressed in {W}. 

This force can be computed using the model that considers both a non-moving base 
platform, frame {B} parallel to {W}, and z B = — g, i.e., 

, f = _M — ^w (120) 

r l«|£ 

P s being the piston potential energy. Using equation (92) it can be computed by: 

p *= m .'g-\i-^\b+'p,J (i2i) 

The gravitational component of the actuating forces due to each piston, t s , ,, is 

It should be noted that the base platform motion originates new inertial contributions to the 
parallel manipulator dynamic model, expressed by equations (38), (67), (84), (105) and (118). 
These contributions should the added to the corresponding ones resulting from the model 
that considers a fixed-base: equations (37), (66), (83), (104) and (117). Regarding the 
gravitational part of the dynamic model, the base platform motion modifies the 
gravitational force components, resulting in the equations (42), (88) and (122), which can 
also be added to the previous ones. 

Computational efficiency of the proposed model has been evaluated by counting the 
number of scalar operations needed in the calculations (sums, multiplications and 
divisions). For this purpose, the Maple® software package was used. The results were then 
compared with the ones obtained by using the Lagrange formulation. The generalized 
momentum approach resulted in a much more efficient dynamic model. Regarding the total 
number of sums and multiplications involved in the two models, the ratio is five, 
approximately. This might be a great advantage if real-time simulation and control is 
needed. 

4. Conclusion 

A parallel manipulator is a complex multi-body dynamic system having several closed 
loops. Typically, it is composed of a (usually) fixed base platform and a moving pay load 
platform, connected by at least two independent open kinematic chains. Dynamic modelling 
of parallel manipulators presents an inherent difficulty. Despite the intensive study in this 
topic of robotics, mostly conducted in the last two decades, additional research still has to be 
done in this area. 



Fast Dynamic Model of a Moving-base 6-DOF Parallel Manipulator 379 

In this book chapter an approach based on the manipulator generalized momentum was 
explored and applied to the dynamic modelling of a Stewart platform manipulator. The 
system dynamic equations were obtained for the general case of a non-fixed base platform. 
It was shown the base platform motion originates new inertial force contributions to the 
parallel manipulator dynamic model. Compact analytical expressions for these contributions 
were presented and it was shown they can be easily added to the corresponding ones 
resulting from the model that considers a fixed base. On the other hand, regarding the 
gravitational part of the dynamic model, the base platform motion modifies the 
gravitational force components derived when considering a fixed base platform. Analytical 
expressions for these components were also presented. Computational efficiency of the 
dynamic model was evaluated by counting the number of scalar operations that are needed. 
The results were then compared with the ones obtained by using the Lagrange formulation. 
The generalized momentum approach results in a much more efficient dynamic model. 
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1. Introduction 

In comparison to classical serial mechanisms parallel kinematic machines (PKM) provide a 
higher accuracy, a higher stiffness, and higher dynamic properties. However, parallel robots 
suffer from the presence of singularities within their workspace (Gosselin & Angeles, 1990). 
In such configurations the moving platform gains at least one degree of freedom (DOF) and 
the actuation forces become (in theory) infinite. As a result, the kinematic structure can be 
damaged or even destroyed. Additionally, several performance indices, e.g. the achievable 
accuracy, are directly related to the singularity loci (Kotlarski, Abdellatif & Heimann, 2008). 
The closer the endeffector (EE) is 'located' to a singularity the higher is the pose error resulting 
from the influence of active joint errors, e.g. from limited encoder resolution. 
In order to minimize the singularity loci of parallel mechanisms and to increase their per- 
formance, e.g. their achievable accuracy, redundancy can be used (Merlet, 1996). Two re- 
dundancy approaches are established for PKM, actuation redundancy and kinematic redun- 
dancy (Kock & Schumacher, 1998; Wang & Gosselin, 2004). Actuation redundancy can be 
realized whether by adding a kinematic chain to the mechanism or by actuating a passive 
joint. Amongst others, it reduces singular configurations and leads to internal preload that 
can be controlled in order to prevent backlash (Kock, 2001). However, the control of such 
mechanisms is a challenging task (Miiller, 2005). Furthermore, an additional kinematic chain 
mostly reduces the total workspace. Therefore, kinematic redundancy is proposed realized 
by adding at least one actuated joint to one kinematic chain (Cha et al., 2007; Mohamed & 
Gosselin, 2005). 

It is well known that the singularity loci as well as the achievable accuracy are greatly affected 
by the geometrical parameters of a mechanism (Kotlarski, de Nijs, Abdellatif & Heimann, 
2009; Merlet & Daney, 2005), and are therefore highly dependent on the mechanism's ac- 
tual configuration. In this chapter, as examples, kinematically redundant versions of the 
well known planar 3RRR and 3RPR mechanisms (Gosselin & Angeles, 1988; Zein et al., 2006) 
are considered. In each case, an additional prismatic actuator is added to an arbitrary base 
joint. The introduced mechanisms are denoted as 3(P)RRR and 3(P)RPR. Thanks to the ad- 
ditional prismatic actuator, the inverse displacement problem has an infinite number of so- 
lutions (Ebrahimi et al., 2007). Hence, reconfigurations of the mechanisms can be performed 
selectively in order to avoid singularities and to affect their performance directly (Kotlarski, 
Do Thanh, Abdellatif & Heimann, 2008). It is important to note that with respect to the work 
of Arakelian et al. (Arakelian et al., 2008), kinematic redundancy can be used to rather change 
the geometrical parameters of a mechanism than its basic structure. This can be done at the 
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task planning stage or while operating the manipulator using several different strategies (see 
Sec. 3.2). 

Here, the key idea is to change the position of the redundant actuator in a discrete manner 
while operating the mechanism, in particular just before shifts in direction of the moving 
platform. This allows for optimizing the accuracy of the manipulator for a given trajectory 
segment. After each switching operation, i.e. after each reconfiguration, the additional pris- 
matic actuator is supposed to remain locked. Therefore, the joint clearance as well as the 
control error corresponding to the redundant actuator can be minimized. The resulting set of 
discrete actuator positions is called the switching pattern. The optimization of the switching 
patterns is achieved according to a performance index denoted as the gain of the maximal 
homogenized pose error (see Sec. 3.2). 

The rest of this chapter is organized as follows. In Sec. 2 the geometric and the inverse kine- 
matic models of the proposed mechanisms are given as well as fundamental definitions re- 
lated to their Jacobian analysis. Sec. 3 clarifies the idea of kinematic redundancy in order to 
avoid singularities and to increase the performance of a PKM. Additionally, it gives a brief 
theoretical overview on the determination of the achievable moving platform pose accuracy 
and introduces the optimization strategy developed for the redundant actuator position. In 
Sec. 4 several analysis examples are presented in order to validate the proposed redundant 
schemes with the optimized switching patterns. It is demonstrated that the kinematically re- 
dundant mechanisms in combination with the developed optimization procedure lead to a 
great improvement in terms of singularity avoidance and, therefore, in terms of accuracy and 
precision. Furthermore, it is shown that the proposed optimization criterion is able to out- 
perform the classical accuracy related performance index, i.e. the condition number of the 
Jacobian matrix. Sec. 5 concludes this chapter. 

2. Kinematically redundant mechanisms 

In the following, both the geometrical and the inverse kinematic models of the exemplarily an- 
alyzed planar, kinematically redundant mechanisms are given along with fundamental defi- 
nitions related to the Jacobian analysis. An additional prismatic actuator is proposed allowing 
one base joint to move linearly. Hence, reconfiguration of the mechanisms can be performed 
selectively while operating the manipulators. 

2.1 Redundant 3(P)RRR mechanism 

In (Kotlarski et al., 2007) the kinematically redundant 3(P)RRR planar mechanism (see Fig. 1) 
was introduced. It is basically similar to the non-redundant 3RRR mechanism studied 
amongst others in (Gosselin & Angeles, 1988). Three kinematic chains G,M,P, (/' = 1,2,3) 
connect the moving platform P1P2P3 to the base G1G2G3. Each kinematic chain consists of 
two links In ar, d U,2- The position of the joints G;, M,-, and P, with respect to the inertial co- 
ordinate frame (CF) is given by gr, = (xq., J/g,) T ' rn i = (*M,-/ J/M,) T / an d Pi = (xp,., yp;) T - 
The base-fixed revolute joints are active while the remaining ones are passive. The orienta- 
tion of the redundant actuator with respect to the x-axis of (CF) is denoted by a. Positions 
referenced with respect to the platform fixed coordinate frame (CF) E are marked with ('). 
In the following the configuration of the moving platform is given by 

x = (xe, Ve, <p ) T , (1) 

where x^ and i/p represent the point of origin of the platform fixed coordinate frame (CF) E 
with respect to (CF) and <p is its orientation. The mechanism is driven by the four actuators. 
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Fig. 1. Kinematically redundant 3(P)RRR mechanism 



Therefore, the system input is given by the according actuator coordinates 



= (0i, 2 , 



S)> 



2.1.1 Inverse kinematics 

For each kinematic chain i the geometric constraints can be written as 



3/Pi 



X G> 



+ 



cos(0,) cos(0,- + Ipi 



hi 



(2) 



(3) 



sin(0,) sin(0,- + ipj 
where the position of the moving platform's passive joints with respect to (CF) is defined as 



J/P, 



n 



cos((f>) — sin(cp) 
sin(c^) cos(c^) 



y-Pi 



In the presented redundant case the position of Gi depends on the actuator position 5: 



Vg, 



VG, 



S=0m 



S cos(a) 
S sin (a) 



From (3) the passive joint angles tpj can be determined: 

' r 2 , y 2 _ ;2 _ /2 
x GP f + ^GP, l i,\ l i,2 



xpi = ± arccos 



2 til 



U 'i,2 



(4) 



(5) 



(6) 



where x G p. = Xp / — Xq. and i/ G p. = yp. — y G . The active joint angles 0, are finally obtained 
using (3) and (6): 



0, = arctan 



{h,l+h,2 cos(^)) y G p, - {k,2 sm(fi)) *GP, 
('/,! + l i,2 cos(ipi)) *gp, + (/, /2 sin(t/>,)) y GP . 



(7) 
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Additionally, for simplification reasons, the angles f; are defined as the counterclockwise an- 
gles that the passive links form with the x-axis: 

x. = arctan f yggj i l i,2 + U,i cos(i/>,)) + (/,,i sin(i/>,-)) x GP . \ 
\*GP, ('(,2 + ?;,i cos(i/),)) - (k,x sm(fi)) y GP . y ' 

2.1.2 Jacobian formulation 

Several performance criteria and indices, e.g. the achievable accuracy, can be calculated based 
on the Jacobian matrices of a PKM (see Sec. 4). After summing the squares of (3) the Jacobians 
can be obtained by a derivation of the resulting inverse kinematic equations /, 

fi = = (xp, - X Gi - /,,i cos(0,)) 2 + (yp, - VG, - hi sin(0,)) 2 - %, (9) 

with respect to time (Gosselin & Angeles, 1990): 

^-^+^^ = *» Ax + B0 = 0. (10) 

6x 68 

For the 3(P)RRR mechanism the direct and the inverse Jacobian matrices A and B result to 

«22 «23 / B=[ fe 1, (11) 



"32 fl 33 



fell 








''14 





&22 














*>33 






with (for/ = 1,2,3) 



and 



a i\ — Z P, — Z G, — 'i,l cos ^/' 

«£2 = yp, - ye, - k,l sine,-, ^ 

«J3 = - a i\ ( X P, sin (</') + 3/p, COs(#) J + «a (x Pi cos (if) - y P sin(<^ 



(13) 



&fl = *i,l («fl sin(0,) - fl, 2 cos(0,-)) , 
&J4 = — («u cos(a) + «i2 sin(a)) . 



As long as the Jacobian A is nonsingular, its inverse A can be determined analytically: 

^ / ti^U22 — C>32 a 23 — ( fl 33 fl 12 ~ a 32 a \?>) fl 23 fl 12 ~ fl 22 fl 13 \ 

" A_1 = detTAY _ ( fl 33«21 -«31 fl 23) «33«U - «31«13 ~(«23«11 ~ «21«13) • (I 4 ) 

\ «32«21 - «31«22 -(«3a«ll-«31«12) fl 22flll - «21«12 / 

It will be useful for calculating the achievable accuracy as demonstrated in Sec. 3.1. 

2.2 Redundant 3(P)RPR mechanism 

Additionally, the kinematically redundant 3(P)RPR planar mechanism presented in Fig. 2 is 
considered. It was firstly introduced in (Kotlarski, Abdellatif, Ortmaier & Heimann, 2009). It 
is based on the well known 3RPR mechanism (Zein et al., 2006). Three kinematic chains G;Pi 
(i = 1,2,3) consisting of active prismatic joints connect the moving platform P1P2P3 to the 
base G1G2G3. In the following, notations and definitions similar to the 3(P)RRR mechanism 
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Fig. 2. Kinematically redundant 3(P)RPR mechanism 



and already introduced are not mentioned again. 

The system input is given by the four actuator coordinates 

0= (Pi, Pi, P?,, £) T , 
where p, defines the lengths of the kinematic chain i. 

2.2.1 Inverse kinematics 

The geometric constraints of the 3(P)RPR mechanism can be written as 



T P, 

!/p, 



XG, 



cos(£j) 

~ Pi { MSO 



where the passive joint angles £, are obtained by 



arctan 



V*GP, 



(15) 



(16) 



(17) 



From the Euclidean norm of the vector connecting point G, to point P, the lengths of each 
kinematic chain i are obtained 



Pi — X GP, + 3/GP, • 



(18) 



2.2.2 Jacobian formulation 

Similar to (11) and using /, (cp. (18)) 



fi = = 1-gp,. + Vgp, - Pi 



(19) 



for the 3(P)RPR mechanism the elements of the direct and inverse Jacobian matrices A and B 
result to 



an a\i a 13 

fl 2 i a 2 2 "23 
fl 31 fl 32 «33 



/ fon b u 
B = b 22 
V b 33 



(20) 
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with (for/ = 1,2,3) 



H\ = *GPi> 

%q = — an ( x' F . sin((p) + y?, cos(ip) ) + fl/2 ( x' P . cos(cp) — \f 7 . sin(</») 



(21) 



and 



hi = - Pi, 
b\i = — (flu cos(a) + «i2sin(a)) . 



(22) 



3. Singularity avoidance and accuracy improvement using kinematic redundancy 

The condition for type-two singularities of planar mechanisms can be formulated geometri- 
cally (Hunt, 1978; Yang et al., 2002). For the here treated and common case of revolute passive 
joints at the moving platform, a pose of the robot is singular if the three lines passing through 
the passive links of the kinematic chains (passive lines) intersect at a common point or are all 
parallel. Thanks to the kinematic redundancy, the direction of the passive lines and, therefore, 
the Jacobians' elements can be directly affected. As a result, the singularity loci change as 
shown in Fig. 3, exemplarily for the introduced kinematically redundant versions of the pla- 
nar 3RRR (top) and 3RPR (bottom) mechanisms with a base joint mounted on an additional 
prismatic actuator. 






Fig. 3. Variation of the singularity loci (solid red) due to the redundant actuator configuration, 
i.e. the base joint position; top: redundant 3(P)RRR, bottom: redundant 3(P)RPR 

The use of kinematic redundancy to avoid singularities is demonstrated in Fig. 4, left. The 
given path would cross a singularity for the symmetric, i.e. the 'classical', configuration. By 
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moving the redundant actuator towards the right, the singularity loci could be completely 
removed. In case of the mechanism performance, e.g. in case of the achievable accuracy, sim- 
ilar effects are given. Regions suffering from high pose errors, i.e. workspace regions that do 
not provide a certain desired accuracy, can be 'moved' and, therefore, avoided when follow- 
ing a desired trajectory as shown in Fig. 4, right. Hence, the achievable accuracy increases 





Fig. 4. Trajectory (solid black) going through a singular configuration (left, solid red) / a 
region where a certain accuracy in one or more directions is not given (right, yellow); a recon- 
figuration (from the left to the right) allows to follow the desired path 



significantly as demonstrated in Sec. 4. 

3.1 The moving platform's pose error 

Due to several factors, like manufacturing errors, joint clearance, and active joint errors, the 
pose of the moving platform can be provided only within a given accuracy. An approach to 
determine the achievable accuracy of a PKM while considering any kind of uncertainties can 
be found in (Kotlarski, Abdellatif, Ortmaier & Heimann, 2009). Referring to (Merlet, 2006a), 
the active joint errors, e.g. the limited resolution of the encoders, are the major sources of 
error in a calibrated and precisely manufactured PKM. Therefore, the analysis is focussed on 
the achievable accuracy of a moving platform in the presence of active joint errors only. 
By rewriting the velocity equation (10) in incremental form, an approximation that relates the 
active joint errors (the input error) AO to the pose error (the output error) Aa; is obtained: 



A Ax + BAG o- AAx = -BA0 



Ax 



i-i 



-B)A6 = -JA6. 



(23) 



Using (23) and incorporating the fact that \ab\ = \a\\b\ and \a + b\ < \a\ + \b\ V (#,&) € R the 
maximal pose error vector Ax can be calculated by 



Ax 




I Jill I/12I l/l3l l/l4l 
1/21 1 I/22I Ifel I/24I 
1/31 1 Ifel Ifel Ifel 



/ |Afli| \ 
|A0 2 | 

|A0 3 | 
V |A*| J 



> \Ax\ 



(24) 



The Jacobian element of row i and column j is denoted as /,,. Since the Jacobian matrix J 
highly depends on the actuator position 5, i.e. on the robot geometry, the additional DOF of 
the proposed kinematically redundant mechanisms can be used to affect the Jacobian elements 
and, therefore, the robot accuracy directly. Fig. 5 shows the elements of Aa; with respect to 
the actuator position 5 for the chosen kinematically redundant mechanisms (left: 3(P)RRR, 
right 3(P)RPR) and an arbitrary constant EE pose x. The elements of the active joint error 
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vector A9 are taken from data sheets of available actuators. The authors forbear from giving 
the concrete parameters, i.e. the robot geometry, the EE pose, and the active joint errors, 
because the characteristics of Ax with respect to 3 is similar in all cases. The dependency of 




0.25 



-©- 
< 




<5[m] 

(a) 3(P)RRR mechanism 



0.15 




< 



(b) 3(P)RPR mechanism 



Fig. 5. Positioning error Ax and Ay (solid gray), the overall translational error Axy (solid black) 
and the orientational error Alp (solid red) with respect to the redundant actuator position 3 and 
for a constant EE pose x 



the achievable accuracy on the redundant actuator position is well noticeable. Additionally, 
it can be seen that the elements of the maximal pose error Ax, Ay, and A(p as well as the 
overall translational error Axy = || (Ax, Ay) H2 have similar minima. Hence, in most cases, 
the maximal accuracy of each DOF can be increased for almost identical actuator positions 3 
by an appropriate reconfiguration of the mechanism. Therefore, an optimization procedure is 
required in order to find the best solution for 3. 

3.2 Optimization of the redundant actuator position 

The optimization of the redundant actuator position 3 can be performed based on two main 
strategies: a classical continuous optimization and a selective discrete optimization. The latter 
is the key idea of this chapter and is discussed in the following. 

Undoubtedly, a continuous optimization leads to an instantly influenceable, i.e. maximal 
achievable, accuracy. In contrast to the mentioned advantage, it results in a more challeng- 
ing task concerning the robot control and usually in a higher energy demand. The proposed 
approach is based on the optimization of 3 in a discrete manner while operating the system. 
Therefore, the trajectory is divided into segments. The starting and final points of the seg- 
ments are certain poses, e.g. shifts in direction. Appropriate constant values of the actuator 
position 3 corresponding to the different segments of the desired trajectory are determined. 
The resulting set of discrete actuator positions is called the optimized switching pattern. While 
moving along the desired trajectory, the position of the redundant actuator is changed accord- 
ing to the switching pattern. This allows for the reconfiguration of the mechanism to influence 
its accuracy for a given path segment. While performing a reconfiguration the pose of the 
moving platform is kept constant. After each switching operation, e.g. while moving along a 
trajectory segment, the additional prismatic actuator is supposed to remain locked. Therefore, 
compliance, e.g. resulting from joint clearance, as well as the control error corresponding to 
the redundant actuator are minimized. 

In order to further minimize the switching operations the mentioned discrete optimization, 
i.e. the 'main idea', can be additionally modified in several ways. One possibility is to only 
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change the redundant actuator position once before starting the desired movement. As a re- 
sult, the number of reconfigurations is minimized. But, regarding complex trajectories, i.e. 
trajectories going through a large area of the robot workspace, this may lead to an unaccept- 
able performance, e.g. with respect to the accuracy. Thus, another possible modification is to 
perform a reconfiguration only if the mechanism is unable to perform the desired operation, 
e.g. following a singularity-free trajectory and providing a certain accuracy, in its current con- 
figuration (Kotlarski, Do Thanh, Abdellatif & Hermann, 2008). Therefore, before moving the 
EE, for the upcoming trajectory segment the required performance criteria have to be calcu- 
lated. If any criteria is less than its corresponding threshold a reconfiguration of the mech- 
anism has to be performed. The mentioned modification of the proposed selective discrete 
optimization strategy further reduces the inconvenient switching operations and guarantees 
a desired performance. Nevertheless, in this chapter it is focussed on the main idea of the dis- 
crete reconfiguration strategy without any of the modifications mentioned in this paragraph. 
It mostly clarifies the influence of the additional prismatic actuator which is the authors' main 
purpose. 

The optimization can be realized with respect to several criteria and performance indices: a 
well accepted criterion is the condition number (in general the two-norm condition number) 
of the Jacobian matrix k( J) and its inverse t] = k _1 called dexterity. In (Gosselin, 1992) it is 
defined as: 

k(J)=k(J- 1 ) = \\J- 1 \\ 2 \\J\\2, 1<k<oo, (25) 

where k = 1 represents an isotropic configuration without an amplification of the active joint 
error A0 and k = oo represents a singular configuration with an infinite amplification of AO 
leading to (in theory) an infinite Ax. However, the moving platforms of the considered mech- 
anisms have two translational as well as one rotational DOE As a result, the Jacobian matrix J 
is not homogeneous in terms of physical units. Therefore, the value of the condition number 
depends on the unit choice. Hence, a modification of the Jacobian matrix is required in order 
to obtain appropriate values for k. Amongst others, the homogeneity can be achieved by trans- 
forming the moving platform velocity x into the linear velocity x^ = (xp,, l/p,, ip 2 , J/p 2 ) of 
two arbitrary points Pj and P2 (Pond & Carretero, 2006). Therefore, a transformation matrix 
Q has to be found that satisfies the following equation: 

±h = Qx, (26) 

where the subscript 'h' indicates homogeneous. But, instead of describing a manipulator with 
three DOF by the four parameters x^, a reduction of the terms describing the velocities of 
the moving platform to three can be performed (Gosselin, 1992). As a result, the dimension 
of the Jacobian matrix J remains constant. Therefore, a coordinate frame (CF)p h , located at 
Pi, is attached to the moving platform such that its x-axis passes through Pj and P2. For the 
proposed mechanisms, by choosing x^ = (jp, yp, J/p 3 ) T (cp. Fig. 1 and Fig. 2), the modified 
transformation matrix Q results to: 



Q 



cos /3 sin /3 

— sin /3 cos /3 

— sin/3 cos/3 1 1 333 1 1 2 



(27) 



where the angle /3 gives the orientation of (CF) to (CF)p h . It is important to note that the 
points Pi and P2 can be chosen arbitrary as long as they fulfill the mentioned characteris- 
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tics (Gosselin, 1992). The homogenized Jacobian matrix J^ can finally be determined us- 
ing (23) and (26): 

J h = QJ. (28) 

Hence, an optimization of the actuator position S can be performed by a minimization of the 
condition number k(Jj 1 ) and by a maximization of the dexterity j/(Jj,), respectively: 



^opt = arg I min x(J h ) \ = S opt = arg [ max t]( J h ) j . (29) 

As demonstrated by Merlet (Merlet, 2006b) and shown later in Sec. 4 the condition number 
does not necessarily exhibit a complete consistent behavior with respect to the pose error of 
a robot. Therefore, an optimization of the actuator position S based on minimizing the two- 
norm of the maximal homogenized pose error Ax^ is proposed: 



7(Aa; h ) = 1 1 Ax 



h|| 2 



l/hul 

l/hj 

l/h 31 l 


l/hj 
l/h 22 l 


l/hj 
l/hj 
l/hj 


l/hj \ 
l/hj 
l/hj / 


/ |A0i| \ 

|A0 2 | 

|A0 3 | 
V |A<5| J 



(30) 



where the elements of the active joint error vector AO, i.e. their limited resolutions, are well 
known from the data sheets of the actuators. This index is called the gain j(Ax^) of the 
maximal homogenized pose error Aa^. Although the influence of the prismatic actuator joint 
error AS on the pose error Ax is small only (see Sec. 4) it should not be neglected. The cost 
function to be minimized results to: 



S opt = arg I min 7(Aa; h ) j . (31) 

There might be trajectories, e.g. regarding special applications, for which a high accuracy is 
required in certain DOF only. In this case, the optimization criterion (31) can be adopted such 
that the corresponding elements (or a single element) of Aajj, are solely minimized. 

4. Accuracy analysis - numerical results 

Several examples are presented in order to validate the proposed redundant scheme with the 
developed optimized switching patterns. The advantage of the approach is verified for differ- 
ent trajectories. Additionally, the influence of the redundant prismatic actuator on the mov- 
ing platform pose accuracy is demonstrated. Moreover, in order to further confirm the results 
given in Sec. 4.1, the useable workspace, i.e. the singularity-free part of the workspace pro- 
viding a certain performance, of the considered mechanisms is determined (and compared). 

4.1 Simulation of single trajectories 

Accuracy analysis along selected simulated trajectories were performed. The geometrical 
parameters of the analyzed kinematically redundant mechanisms and their non-redundant 
counterparts are given in Table 1. In the redundant case, one prismatic actuator is attached 
to Gj of the basic structure. Keeping the design space in mind the orientation of the redun- 
dant actuator was set to a = 0° . At this point, it is important to note that the design of the 
additional prismatic actuator, i.e. its stroke as well as its orientation, was more or less cho- 
sen intuitively. Future work will deal with an optimization of the parameters related to the 
redundant actuator. 
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i = 1 


i' = 2 


2 = 3 


*G,-[mJ 


0.6 





1.2 


3/G, M 


VT7/5 








x' v [m] 





-0.125 


0.125 


y' p . [m] 





-\/3/8 


-a/3/8 


k,i H 


0.6 


0.6 


0.6 


'/,2[m] 


0.6 


0.6 


0.6 


|Oi,minH 


0.1 


0.1 


0.1 


|0/,max[m] 


1.2 


1.2 


1.2 



3(P)RRR & 3(P)RPR 

3(P)RRR 

3(P)RPR 

Table 1. Design parameters of the analyzed 3(P)RRR and 3(P)RPR mechanisms (—0.5 m < S < 
0.5 m) 



4.1.1 Redundant 3(P)RRR mechanism 

First, the accuracy analysis of the kinematically redundant 3(P)RRR mechanism is performed. 
Exemplarily simulation results of the three triangular trajectories (t\, t\\, t\\\) shown in Fig. 6 
are presented. In order to clarify the effectiveness of the proposed concept the trajectories were 
chosen within the workspace of the mechanisms (solid black) such that the non-redundant 
mechanism (5 = m) does not pass any singular configurations when (p = 0° . Without loss of 
generality the regarded 3RRR-based mechanisms are in the following assumed to remain in 
the same working mode which is shown in Fig. 1. 





(a) 3(P)RRR(cf>= -30°) 



(b) 3(P)RRR (f = 0°) 




(c) 3(P)RRR((l> = 30 o ) 



Fig. 6. Exemplarily chosen trajectories t\, t\\, tm; (solid gray) for the 3RRR-based mechanisms, 
the solid red lines represent the singularity loci within the workspace (solid black) 

The EE was moved counterclockwise along the depicted trajectories with a constant orienta- 
tion. The trajectories were divided such that each side of a triangular represents a segment. 
Hence, at every corner Cy, Cy, and cy (z = I, II, III) the position of the redundant actuator 5 
is switched according to the optimized switching pattern. During each switching operation 
the moving platform pose is kept constant. The optimization was performed based on the 
introduced cost functions (29) and (31). Even though the prismatic joint is locked between 
two switching phases, its joint error, e.g. the limited resolution of the encoder, has to be taken 
into account in order to obtain a realistic and practical accuracy analysis. Therefore, the active 
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joint errors were chosen based on data sheets of commercially available standard actuators 
to A0(3(P)RRR) = (0.025°, 0.025°, 0.025°, 40/<m) T . It is important to note that in the non- 
redundant case the last element of A0 vanishes. 

In Fig. 7 the optimized switching patterns 6 „t of the actuator position S as well as the re- 
sulting mechanism pose errors Axy and A(p are presented. The EE was moved along tra- 
jectory t\ with a constant orientation of (p = —30°, </> = 0°, and <p = 30° denoted as 
t\{— 30°), fi(0°), and fj(30°), respectively. The distance the EE moved along the trajectory 
is denoted as s. A significant improvement of the accuracy due to the kinematic redundancy 
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s 
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(d) Axi/(f I (-30°)) 




(e) Axy(h(Q°)) 



(f) Axi/^Su )) 




0.05 



Cll 



Cl,2 



C I,3 



(g) A^(ti(-30°)) 







^ 







Cl.l Ci,i 



C I,2 



Cl,3 



(h) A</.(f I (0°)) 




Ct 1 Cl i 



fl.2 



CI,3 



(i) A<f.(f,(30°)) 



Cl,l 



Fig. 7. Simulation results while moving along trajectory tj(— 30°) (left), £i(0°) (center), and 
fl(30°) (right); solid gray: non- redundant mechanism; dashed black: optimized redundant 
mechanism using tj(J^); solid red: optimized redundant mechanism using 7(Aaj[ 1 ) 



is well noticeable. E.g. regarding fj(— 30°) and ti(0°), the maximal pose error occurring close 
to Ct 2 is minimized by a reconfiguration of the mechanism according to the optimized switch- 
ing patterns. Fig. 7 shows that both optimization criteria ()/( J^) and ^(Ax^)) lead to similar 
switching patterns and to similar achievable accuracies. In Table 2 an overview of the maxi- 
mal errors of the three triangular trajectories shown in Fig. 6 are given. In order to quantify 
the accuracy improvement the maximal translational Axy max and rotational error A(p max of the 
moving platform over a complete trajectory was determined. The values represent the achiev- 
able accuracy of the associated mechanism. Additionally, the percentage increase /decrease of 
the kinematically redundant PKM in comparison to its non-redundant counterpart is given. 
Significant improvements of the achievable accuracy are well noticeable in most cases. Fur- 
thermore, e.g. for frn(30°), it can be seen that an optimization based on the gain y(Ax^) leads 
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ti(<p) 


Value 


3RRR 


3(P)RRR 


using rj(J h ) 


using y(Ax h ) 


(l(-30°) 


A *J/max t mm l 
A tfWx [°] 


7.13 
1.93 


1.34 (-88.3%) 
0.23 (-81.2%) 


1.34 (-88.3%) 
0.23 (-81.2%) 


fl(0°) 


A *>/max [mm] 


1.44 
0.36 


1.02 (-28.8%) 
0.21 (-42.3%) 


0.91 (-36.9%) 
0.16 (-57.2%) 


fi(30°) 


Ax V ma x [mm] 
A <Pmax n 


0.90 
0.32 


0.90 (-0.5%) 
0.32 (+2.2%) 


0.81 (-9.5%) 
0.31 (-2.6%) 


tn(-30°) 


A *»/max [mm] 

A^ max n 


oo 
oo 


0.69 (-) 
0.14 (-) 


0.58 (-) 
0.12 (-) 


tn(0°) 


A *>/max [mm] 
A tfW [°] 


3.25 
1.50 


0.75 (-77.1%) 
0.22 (-85.3%) 


0.69 (-78.9%) 
0.22 (-85.3%) 


f n (30°) 


A *y m ax [mm] 
A <?max [°1 


0.63 
0.37 


0.70 (+10.3%) 
0.43 (+14.5%) 


0.68 (+6.8%) 
0.40 (+7.6%) 


fni(-30 o ) 


A *J/max [mm] 
A <?max [°] 


0.57 
0.30 


0.48 (-15.3%) 
0.26 (-12.0%) 


0.48 (-15.3%) 
0.26 (-12.0%) 


fm(0°) 


A ^y m ax [mm] 
A tfWx [°] 


0.70 
0.41 


0.86 (+22.8%) 
0.31 (-25.0%) 


0.60 (-14.9%) 
0.35 (-13.3%) 


fm (30°) 


A ^y m ax [mm] 
A <?max H 


1.40 
0.41 


1.43 (+2.2%) 
0.44 (+7.0%) 


1.10 (-21.6%) 
0.35 (-14.9%) 



Table 2. Redundant 3(P)RRR mechanism: maximal translational 
A< fmax °f me moving platform while moving along trajectory t\, t 



Axy max and rotational error 
PI, an d fin 



to more appropriate switching patterns in comparison to an optimization based on the condi- 
tion of the Jacobian rj(J^). Regarding fri(30°), due to the additional active joint error AS there 
might be trajectory segments suffering from a decreased performance when using the pro- 
posed discrete optimization, i.e. the proposed switching patterns. This could be avoided using 
a continuous optimization. However, due to the mentioned advantages of the discrete switch- 
ing patterns and due to the minimal decrease of the achievable accuracy only (Axy : 0.05 mm 
and Aip : 0.03°), the authors still propose the selective discrete optimization of the redundant 
actuator position. 

4.1.2 Redundant 3(P)RPR mechanism 

Similar to Sec. 4.1.1, an accuracy analysis of the kinematically redundant 3(P)RPR mechanism 
is performed. Exemplarily simulation results of the three triangular trajectories (tj, £n> fm) 
which are shown in Fig. 8 are presented. In the following, facts and definitions similar to the 
analysis of the 3(P)RRR mechanism and already introduced are not mentioned again. Based 
on the data sheets of commercially available standard actuators, the active joint errors were 
chosen to A0(3(P)RPR) = (0.2mm, 0.2mm, 0.2mm, 40;/m) T . As well, in the non-redundant 
case the last element of AO vanishes. 

In Fig. 9 the optimized switching patterns <W,t of the actuator position 5 as well as the resulting 
pose errors Axy and A<^> of the mechanisms are presented. Again, the EE was moved counter- 
clockwise along trajectory t\ with a constant orientation of (p = —30°, tp = 0°, and <p = 30°. 
It is important to note that the symmetrical non-redundant mechanism suffers from a com- 
pletely singular, i.e. useless, workspace for <p = 0° (indicated by Axy = Aip = oo). This is 
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(a) 3(P)RPR(</> = -30°) 



(b) 3(P)RPR (</> = 0°) 




(c) 3(P)RPR(0 = 3O°) 



Fig. 8. Exemplarily chosen trajectories t\, (rr, frrr (solid gray) for the 3(P)RPR mechanism, 
the solid red lines represent the singularity loci within the workspace (solid black); note: the 
workspace for (p = 0° is completely singular 
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Fig. 9. Simulation results while moving along trajectory £t(— 30°) (left), fi(0°) (center), and 
fl(30°) (right); solid gray: non- redundant mechanism; dashed black: optimized redundant 
mechanism using tj(J^); solid red: optimized redundant mechanism using 7(Aa5j l ) 



not the case for the kinematically redundant 3(P)RPR mechanism where the symmetry can be 
affected, i.e. avoided, thanks to the additional prismatic actuator. Regarding Fig. 9 and Table 3 
similar to the 3RRR-based structure (see Sec. 4.1.1) a significant improvement of the achiev- 
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able accuracy due to the kinematic redundancy is well noticeable. Again, in most cases (except 



W) 


Value 


3RPR 


3(P)RPR 


using rj (J h ) 


using 7(Aa; h ) 


(i(-30°) 


A *3/max [mm] 
A<P max [°] 


4.87 
1.54 


0.70 (-85.7%) 
0.16 (-89.9%) 


0.70 (-85.7%) 
0.16 (-89.9%) 


fi(0°) 


A *y ma x [mm] 
A<P max [°] 




0.90 (-) 
0.53 (-) 


0.90 (-) 
0.48 (-) 


fi(30°) 


A *y max [mm] 
A<P max [°] 


0.97 
0.60 


0.66 (-31.8%) 
0.35 (-41.1%) 


0.66 (-32.5%) 
0.32 (-46.6%) 


fn(-30°) 


A*3/ max [mm] 
A<P max [°] 


0.97 
0.60 


0.66 (-31.9%) 
0.32 (-46.6%) 


0.86 (-11.7%) 
0.35 (-41.9%) 


fn(0°) 


Ax 3/max [mm] 
A<P m ax [°] 


oo 
oo 


0.91 (-) 
0.48 (-) 


0.78 (-) 
0.44 (-) 


f n (30°) 


Ax V ma x [mm] 
A<P max [°] 


4.87 
1.54 


0.70 (-85.7%) 
0.16 (-89.9%) 


0.64 (-86.8%) 
0.15 (-90.2%) 


fin (-30°) 


Ax 3/max [mm] 
A<P max [°] 


0.98 
0.35 


0.93 (-4.6%) 

0.29 (-17.4%) 


0.93 (-4.9%) 
0.28 (-21.2%) 


fm(0°) 


A*3/ max [mm] 
A<P max [°] 


oo 
oo 


oo(-) 
oo(-) 


oo(-) 
oo(-) 


f„i (30°) 


A^y max [mm] 
A<f max [°] 


1.20 
0.41 


0.93 (-22.2%) 
0.27 (-34.1%) 


0.93 (-22.2%) 
0.27 (-34.1%) 



Table 3. Redundant 3(P)RPR mechanism: maximal translational Axy max and rotational error 
A<fmax °f me moving platform while moving along trajectory t\, t\\, and fni 

for trr(— 30°)) the optimization based on the gain 7 (Axj,) leads to more appropriate switching 
patterns (in terms of accuracy improvement) in comparison to an optimization based on the 
Jacobian's condition rj(J^). It is important to note, that even the redundant mechanism suffers 
from singularities (see tni(0° ))• This might be overcome by an optimization of the redundant 
actuator's design which will be subject to future work. 

4.1 .3 Influence of the redundant actuator's joint error 

An additional test was performed to clarify the influence of the redundant prismatic actua- 
tor joint error AS on the moving platform pose error Ax. Therefore, for different AS the EE 
was moved along I(— 30°). The actuator position S was changed according to the optimized 
switching pattern shown in Fig. 7 and Fig. 9 (based on the gain y(Ax^)). The results are pre- 
sented in Fig. 10. The plots clearly demonstrate the marginal influence of AS on Ax when 
realistic values for the remaining active joint errors are chosen (cp. Sec. 4.1.1 and 4.1.2). It can 
be seen that even in the case of an unrealistic high joint error AS a significant increase of the 
mechanism's achievable accuracy in comparison to the non-redundant case is still obtained 
(cp. Fig. 7, left column). 



4.1.4 Switching operations - accuracy progress 

There might be the case that the EE passes a singular configuration while performing a re- 
configuration of the mechanism, i.e. while changing the singularity loci. As a result, the 
performance of the PKM decreases dramatically. Hence, the switching operations have to be 
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Fig. 10. Influence of AS on Ax while moving the EE along trajectory I(— 30°) (solid black: 
AS = fan; solid red: AS = 50 pim; solid gray: AS = 100 jon; solid light gray: AS = 250 fan 



considered within the optimization procedure. While performing a reconfiguration (moving 
S while keeping x constant) the possibility of passing any singularities is taken into account. 
Additionally, configurations of low performance are avoided. Exemplarily, the behavior of 
the achievable accuracy obtained while moving the EE along t\(— 30°) (including the switch- 
ing operations) is given in Fig. 11. It can be clearly seen that the achievable accuracy does 
not increase during reconfigurations of the mechanism. This is valid for all the trajectories 
the authors tested so far. A problem however is the additional operation time necessary to 
follow a desired path. This, i.e. the number of reconfigurations, could be reduced according 
to the modifications mentioned in Sec. 3.2, e.g. only change S once before starting the desired 
movement or if the mechanism is unable to perform a desired operation. Furthermore, the 
switching time itself could be reduced by a 'semi discrete' optimization strategy, e.g. start 
moving S shortly before arriving at the ending point c,- ,• of the segment j of trajectory i. 

4.2 Comparing the useable workspace 

In order to further clarify the effect of an additional prismatic actuator on the mechanism pose 
accuracy, in the following, the size of the useable workspaces w u is determined. The useable 
workspace is defined as the singularity-free part of the total workspace W\ providing a cer- 
tain desired performance, in this case a certain desired accuracy. Mathematically, it can be ex- 
pressed as the largest region where the sign of the determinant of the Jacobian det( A) does not 
change and the output error Ax (23) satisfies any thresholds Ax^ = (Axy^, A</> t h r ) , corre- 
sponding to Axy and Atp. Therefore, the Jacobian determinant as well as the moving platform 
pose error are calculated over the whole workspace. An example clarifying the procedure 
leading to w u is given in Fig. 12. The analyzed workspaces for three different EE orientations 
of the non-redundant 3RRR mechanism (S = m = const.) is given. The green part is the 
largest region where the sign of det( A) does not change whereas the red part is the smallest. 
The black area is the overlayed region where a required performance, i.e. a required accuracy, 
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(a)3(P)RRR:<S O p t ( f i(- 30 °)) 

1.5 i ■ 1 0.3 




(b^PJRPRiMfjf-SO )) 
0.8 i 1 0.2 




(c) 3(P)RRR: Axyihi^O )), A(f>(ti(-30°)) 



(d) 3(P)RPR: Azy(f I (-30°)) / A<p(h(-30°)) 



Fig. 11. Simulation results (including switching operations) while moving along trajectory 
(t(— 30°), reconfigurations are performed based on the gain; left: 3(P)RRR, right: 3(P)RPR, the 
switching operation is marked by the gray background 






(b) f = 0° 



(c) <p = 30° 



Fig. 12. Analyzed workspace of the non-redundant 3RRR mechanism (5 = m = const.); 
green is largest region where the sign of det(A) does not change whereas red is the smallest, 
in the black area the required accuracy can not be provided 



can not be provided. Hence, the green color represents the useable workspace with respect to 
the mentioned requirements. That followed, the connected green area can be determined, i.e. 
the shape as well as the size of the useable workspace. 

Three constant EE orientations <p = {—30°, 0°, 30°} were considered. The design of the ex- 
emplarily chosen mechanisms as well as the input error A9 are equal to the ones chosen in 
Sec. 4.1. The thresholds are set to Axy^ = 0.75mm and A^j^ = 0.5°. The results are given 
in Fig. 13. In case of the non-redundant mechanisms the total and useable workspace Wt and 
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Fig. 13. Total (bold lines, filled dots) and useable (light lines, unfilled dots) workspace of the 
kinematically redundant 3(P)RRR mechanism (left, solid red), the 3(P)RPR mechanism (right, 
solid red), and their non-redundant counterparts (left/right, dotted blue); the dashed red line 
gives the useable workspace of the redundant mechanisms for Ax^r = (0.5 mm, 0.35° ) T 



w u was calculated for different base joint positions G\., i.e. for different but constant S{. The 
solid horizontal lines represent W\ and w u for the redundant case when the base joint G\ can 
be moved linearly for —0.5 m < 6 < 0.5 m. Having a look at Fig. 13 a significant improve- 
ment concerning the workspace areas for all the considered EE orientations is well noticeable. 
Furthermore, for the redundant case the useable workspace for Ax t ^ T = (0.5 mm, 0.35°) 
was determined, i.e. the requested accuracy is increased about one third. It can be clearly 
seen that in this case similar workspace sizes are obtained in comparison the non-redundant 
mechanisms with less accuracy requirements. This further demonstrates the use of kinematic 
redundancy in terms of accuracy improvements. 
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5. Conclusion 

In this paper, the kinematically redundant 3(P)RRR and 3(P)RPR mechanisms were presented. 
In each case, an additional prismatic actuator was applied to the structure allowing one base 
joint to move linearly. After a description of some fundamentals of the proposed PKM, the 
effect of the additional DOF on the moving platform pose accuracy was clarified. An opti- 
mization of the redundant actuator position in a discrete manner was introduced. It is based 
on a minimization of a criterion that the authors denoted the gain y(Axh) of the maximal 
homogenized pose error Aaij v Using several exemplarily chosen trajectories a significant im- 
provement in terms of accuracy of the proposed redundant mechanisms in combination with 
the developed optimization procedure was demonstrated. It could be seen that the suggested 
index 7(Aa;h) leads to more appropriate switching patterns than the well known condition 
number of the Jacobian. Additional simulations demonstrated the marginal influence of the 
redundant actuator joint error AS on the moving platform pose error Ax. 
Furthermore, a comparative study on the usable workspaces, i.e. the singularity-free part of 
the total workspace providing a certain desired performance, of the mentioned mechanisms 
and their non-redundant counterparts was performed. The results demonstrate a significant 
increase of the useable workspace of all considered EE orientations thanks to the applied ad- 
ditional prismatic actuator. 

To further increase the overall and the operational workspace, future work will deal with the 
design optimization of the prismatic actuator, e.g. its orientation with respect to the x-axis of 
the inertial coordinate frame as well as its stroke ('length'). In addition, the simulation will be 
extended to PKM with higher DOF and an experimental validation of the obtained numerical 
results will be performed. 
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1. Introduction 

Kinematic singularities of robot manipulators are configurations in which there is a change in 
the expected or typical number of instantaneous degrees of freedom. This idea can be made 
precise in terms of the rank of a Jacobian matrix relating the rates of change of input (joint) 
and output (end-effector position) variables. The presence of singularities in a manipulator's 
effective joint space or work space can profoundly affect the performance and control of the 
manipulator, variously resulting in intolerable torques or forces on the links, loss of stiffness 
or compliance, and breakdown of control algorithms. The analysis of kinematic singularities 
is therefore an essential step in manipulator design. While, in many cases, this is motivated 
by a desire to avoid singularities, it is known that for almost all manipulator architectures, 
the theoretical joint space must contain singularities. In some cases there are potential design 
advantages in their presence, for example fine control, increased load-bearing and singularity- 
free posture change. 

There are several distinct aspects to singularity analysis — in any given problem it may only 
be necessary to address some of them. Starting with a given manipulator architecture, ma- 
nipulator kinematics describe the relation between the position and velocity (instantaneous 
or infinitesimal kinematics) of the joints and of the end-effector or platform. The physical 
construction and intended use of the manipulator are likely to impose constraints on both the 
input and output variables; however, it may be preferable to ignore such constraints in an 
initial analysis in order to deduce subsequently joint and work spaces with desirable charac- 
teristics. A common goal is to determine maximal singularity-free regions. Hence, there is 
a global problem to determine the whole locus of singular configurations. Depending on the 
architecture, one may be interested in the singular locus in the joint space or in the work space 
of the end-effector (or both). A more detailed problem is to classify the types of singularity 
within the critical locus and thereby to stratify the locus. A local problem is to determine the 
structure of the singular locus in the neighbourhood of a particular point. For example, it may 
be important to know whether the locus separates the space into distinct subsets, a strong 
converse to this being that a singular configuration is isolated. 

Typically, there will be a number of design parameters for a manipulator with given 
architecture — link lengths, twists and offsets. Bifurcation analysis concerns the changes in 
both local and global structure of the singular locus that occur as one alters design param- 
eters in a given architecture. The design process is likely to involve optimizing some desired 
characteristic(s) with respect to the design parameters. 
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The aim of this Chapter is to provide an overview of the development and current state of 
kinematic singularity research and to survey some of the specific methodology and results 
in the literature. More particularly, it describes a framework, based on the work of Miiller 
(2006; 2009) and of the author (Donelan, 2007b; 2008), in which singularities of both serial and 
parallel manipulators can be understood. 

2. Origins and Development 

The origins of the the study of singularities in mechanism and machine research literature go 
back to the 1960s and relate particularly to determination of the degree of mobility via screw 
theory (Baker, 1978; Waldron, 1966), the study of over-constrained closed chains (Duffy & 
Gilmartin, 1969) and the analysis of inverse kinematics for serial manipulators. Pieper (1968) 
showed that the inverse kinematic problem could be solved explicitly for wrist-partitioned 
manipulators, typical among serial manipulator designs. Generally, this remains a major 
problem in manipulator kinematics and singularity analysis. In the context of control meth- 
ods, Whitney introduced the Jacobian matrix (Whitney, 1969) and this has become the central 
object in the study of instantaneous kinematics of manipulators and their singularities — a 
number of significant articles appeared in the succeeding years (Featherstone, 1982; Litvin et 
al, 1986; Paul & Shimano, 1978; Sugimoto et al., 1982; Waldron, 1982; Waldron et al., 1985); 
now the literature on kinematic singularities is very extensive, numbering well over a thou- 
sand items. 

Interest in parallel mechanisms also gained momentum in the 1980s. Hunt (1978) proposed 
the use of in-parallel actuated mechanisms, such as the Gough-Stewart platform (Dasgupta & 
Mruthyunjaya, 2000), as robot manipulators, given their advantages of stiffness and precision. 
In contrast to serial manipulators, where the forward kinematic mapping is constructible and 
its singularities correspond to a loss of degrees of freedom in the end-effector, for parallel ma- 
nipulators, the inverse kinematics is generally more tractable and its singularities correspond 
to a gain in freedom for the platform or end-effector (Fichter, 1986; Hunt, 1983). While screw 
theory already played a role in analyzing singularities, Merlet (1989) showed that Grassmann 
line geometry, which could be viewed as a subset of screw theory (see Section 4.2), is suffi- 
ciently powerful to explain the singular configurations of the Gough-Stewart platform. There- 
after, a Jacobian-based approach to understanding parallel manipulator singularities was pro- 
vided by Gosselin & Angeles (1990), who showed that they could experience both direct and 
inverse kinematic singularities and indeed a combination of these. Subsequently, the sub- 
tlety of parallel manipulator kinematics has become even more apparent, in part as a result of 
the development of manipulators with restricted types of mobility, such as translational and 
Schonflies manipulators (Carricato, 2005; Di Gregorio & Parenti-Castelli, 2002). 
The difficulty in resolving the precise configuration space and singularity locus have meant 
that a great deal of the singularity analysis takes a localized approach — one assumes a given 
configuration for the manipulator and then determines whether it is a singular configuration. 
It may also be possible to determine some local characteristics of the locus of singularities. This 
is remarkably fruitful: by choosing coordinates so that the given configuration is the identity 
or home configuration it is possible to reason about necessary conditions for singularity in 
terms of screws and screw systems. The difficulty that arises in deducing the global structure 
of the singularity locus is that there is no straightforward way to solve the necessary inverse 
kinematics. A good deal of progress can be made in some problems using Lie algebra and 
properties of the closure subalgebra of a chain (open or closed). This approach can be found 
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in (Hao, 1998; Rico et al., 2003) but it appears to fail for the mechanisms dubbed "paradoxical" 
byHerve(1978). 

A number of authors have sought to apply methods of mathematical singularity theory to the 
study of manipulator singularities, for example Gibson (1992); Karger (1996); Pai & Leu (1992); 
Tchoh (1991). A recent survey of this approach can be found in (Donelan, 2007b). There are 
several salient features. Firstly the kinematic mapping is explicitly recognized as a function 
between manifolds, though it may not be given explicit form. Secondly, singularities may be 
classified not only on the basis of their kinematic status but also in terms of intrinsic character- 
istics of the mapping. For example, the rank deficiency (corank) of the kinematic mapping is a 
simple discriminator. More subtle higher-order distinctions can be made that distinguish be- 
tween the topological types of the local singularity locus and enable it to be stratified. Thirdly, 
it provides a precise language and machinery for determining generic properties of the kine- 
matics. 

Following the results of Merlet (1989), another approach has been to use geometric algebra, 
especially in the analysis of parallel manipulator kinematics and singularities. It is a common 
theme that singular configurations correspond to special configurations of points, lines and 
planes associated with a manipulator — for example coplanarity of joint axes or collinearity 
of spherical joints. Such conditions can be expressed as simple equations in the appropriate 
algebra. Some examples of recent successful application of these techniques are Ben-Horin & 
Shoham (2009); Torras et al. (1996); White (1994); Wolf et al. (2004). 

3. Manipulator Architecture and Mobility 

A robot manipulator is assumed to consist of a number of rigid components (links), some pairs 
of which are connected by joints that are assumed to be Reuleaux lower pairs, so representable 
by the contact of congruent surfaces in the connected pair of links (Hunt, 1978). These include 
three types with one degree of freedom (dof): revolute R, prismatic P and helical or screw H 
(the first two correspond to purely rotational and purely translational freedom respectively) 
and three types having higher degree of freedom: cylindrical C with 2 dof, planar E and 
spherical S each with 3 dof. Some manipulators include universal U joints consisting of two R 
joints with intersecting axes, also denoted (RR). 

The architecture of a manipulator is essentially a topological description of its links and joints: 
it can be determined by a graph whose vertices are the links and whose edges represent joints 
(Miiller, 2006). A serial manipulator is an open chain consisting of a sequence of pairwise joined 
links, the initial (base) and final (end-effector) links only being connected to one other link. 
If the initial and final links of a serial manipulator are connected to each other, the result is a 
simple closed chain. This is the most basic example of a parallel manipulator, that is a manipulator 
whose topological representation includes at least one cycle or loop. Note that manipulators 
such as multi-fingered robot hands are neither serial nor parallel — their graph is a tree and 
the relevant kinematics are likely to concern the simultaneous placement of each finger. 
Associated with the architecture is a combinatorial invariant, the (full-cycle) mobility u of the 
manipulator, which is its total internal or relative number of degrees of freedom. This is given 
by the formula of Chebychev-Griibler-Kutzbach (CGK) (Hunt, 1978; Waldron, 1966): 

k k 

jt = n(l - 1) - £> - S t ) = £> - n(/c - / + 1) (1) 

;=1 (=1 

where n is the number of degrees of freedom of an unconstrained link (n = 6 for spatial, n = 3 
for planar or spherical manipulators), k is the number of joints, / the number of links and S{ the 
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dofs of the zth joint. The first expression represents the difference between the total freedom 
of the links and the constraints imposed by the joints. The second version emphasizes that 
the mobility is the difference between the total joint dofs and the number of constraints as 
expressed by the dimension of the cycle space of the associated graph (Gross & Yellen, 2004). 
A specific manipulator requires more information, determining the variable design parame- 
ters inherent in the architecture. The formula (1) is generic (Miiller, 2009): there may be specific 
realizations of an architecture for which the formula does not give the true mobility. For ex- 
ample, the Bennett mechanism consists of 4 links connected by 4 revolute joints into a closed 
chain and is designed so that the axes lie pairwise on the two sets of generators of a hyper- 
boloid. The CGK formula gives ji = 6 X (4 — 1) — 0_i (6 — 1) = —2 but in fact the mechanism 
is mobile with 1 dof. Instances of an architecture in which (1) underestimates the true mobil- 
ity are termed over-constrained. In other cases, there are specific configurations in which \i 
does not coincide with the infinitesimal freedom of the manipulator. This has given rise to 
a search for a more universal formula that takes into account the non-generic cases, see for 
example (Gogu, 2005). It is precisely the discrepancies that arise which correspond to forms 
of singularity that are explored in subsequent sections. 

4. The Kinematic Mapping 

In the robotics literature, the Jacobian matrix for a serial manipulator is the linear transfor- 
mation that relates joint velocities to end-effector velocities. Explicitly, suppose the joint 
variables are q = (^j,...,g/ c ) and the end-effector's position is described by coordinates 
x = (xi,...,X n ). Thus, k is the total number of the joints (or total degrees of freedom, if 
any have > 2 dof) of the joints, while n is the dimension of the displacement space of the 
end-effector, typically either n = 3 for planar or spherical motion or n = 6 for full spatial 
motion. The kinematic mapping is a function x = f(8) that determines the displacement of the 
end-effector corresponding to given values of the joint variables. Then for a time-dependent 
motion described by q = q(f), at a configuration qg = q(frj) sa Y/ 

*fo) = //(qo)q(fo) (2) 

where / = //(qo) is the n x k matrix of partial derivatives of / with z'/th entry dfj/dq;. It is 
important to note that the linear relation expressed by (2) holds infinitesimally; the Jacobian 
matrix is itself dependent on the joint variables. In many practical situations, for example in 
control algorithms, the requirement is to find an inverse matrix for /, which is only possible 
when k = n and the determinant of the Jacobian is non-zero. In the case k > n, the kinematics 
are said to be redundant and one may seek a pseudo-inverse. In the case of wrist-partitioned 
serial manipulators, the Jacobian itself partitions in a natural way and so the singularity loci 
of such manipulators can also be analyzed relatively simply (Stanisic & Engleberth, 1978). It 
is not essential to consider the time-dependence of a motion: from the point of view of the 
manipulator's capabilities, the Jacobian is determined by the choice of coordinates for joints 
and end-effector so the properties of interest are those that are invariant under change of 
coordinates. This is made clearer if the domain and range of the kinematic mapping / are 
properly defined. 

4.1 Displacement groups 

The range is the set of rigid displacements of the end-effector. The rigidity of the links of 
a manipulator, including its end-effector, means that their motion in space is assumed to be 
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isometric (distance between any pair of points is preserved) and orientation-preserving. As- 
suming the ambient space to be Euclidean, the resulting set of possible displacements is the 
spatial Euclidean (isometry) group S£(3) (Murray et al., 1994; Selig, 2005). Composition of dis- 
placements and the existence of an inverse displacement ensure that, mathematically, this set 
is a group. Moreover it can be, at least on a neighbourhood of every point, given Euclidean 
coordinates, and hence forms a Lie group having compatible spatial or topological (differen- 
tiable manifold) and algebraic (group) structures. The number of independent coordinates 
required is the dimension of the Lie group and SE(3) is 6-dimensional. It is, via choices of an 
origin and 3-dimensional orthonormal coordinates in the ambient space and in the link, iso- 
morphic (that is to say topologically and algebraically equivalent to) to the semi-direct product 
SO(3) K R , where the components of the product correspond to the orientation-preserving 
rotations about a fixed point (the origin) and translations, respectively. 

For planar manipulators, analogously, the Euclidean isometry group is the 3-dimensional 
group SE(2) = SO(2) x R . Every element of the rotation group SO(2) may be written, with 
respect to given orthonormal basis for R , in the form: 

cos 8 — sin 9\ 

sing cosS ) { ' 

where 9 measures the angle of counter-clockwise rotation. In this case, 8 is a coordinate for 
the 1-dimensional group SO(2) which can, in fact, be used globally, though it is not one-to- 
one. Indeed, it is clear that SO(2) is, in a topological sense, the same as a unit circle, denoted 
S 1 and 8 and 9 + Ipn represent the same point for any integer p. Topologically, SO(3) is 
3-dimensional projective space. Coordinates may be chosen in a number of ways. The dimen- 
sions correspond locally to the rotation about each of three perpendicular axes, or one can use 
Euler angles. Alternatively there is a 2:1 representation by points of a 3-dimensional sphere, 
and unit quaternions or Euler-Rodrigues parameters are often used. 

Regarding the joint variables, the motion associated with each Reuleaux pair corresponds to a 
subgroup of SE (3) of the same dimension as its degrees of freedom (Herve, 1978). In particular 
R, P and H joints can be represented by one-dimensional subgroups of the Euclidean group. 
For an R joint, the subgroup is topologically S , while for P and H joints the group is R. For 
an S joint, the subgroup is (a copy of) SO(3); for C and E joints the subgroups are equivalent 
to S x R and R x R respectively. Depending on the architecture of the manipulator, its joint 
variables therefore lie in a product of components, each of the form either S , R or SO (3), and 
this product Q, say, forms the theoretical domain of the kinematic mapping. As mentioned 
previously, however, there are in practice almost certainly restrictions on the joint variables 
so that the actual domain is some subset of the theoretical joint space. The set Q is also a 
manifold for which the joint variables give coordinates, say q = (c\\, . . . , q^), at least locally 
though not necessarily in a one-to-one manner for the whole space at once. 
The kinematic mapping has the form of a function / : Q — > G, where Q is the joint space and 
G the displacement group for the end-effector, are well-defined manifolds. Local coordinates 
enable / to be expressed explicitly as a formula. While G is usually taken to be SE(3), for 
spherical manipulators in which there is a fixed point for the end-effector G = SO(3). For a 
robot hand or multi-legged walking robot, G may be a product of several copies of SE(3). For 
a positional manipulator, for example a 3R arm assembly that determines the wrist-centre for 
a wrist-partitioned serial manipulator, the range is simply R . 
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4.2 Infinitesimal kinematics 

Associated with a given point in either of these spaces i) 6 Q, g 6 G, is its tangent space, 
denoted T„Q, TgG, consisting of tangent vectors of paths through that point. The tangent 
spaces are vector spaces of the same dimension as the corresponding manifold. In terms of a 
choice of local coordinates q on a neighbourhood of q E Q and x near g £ G, these tangent 
vectors will correspond to the vectors q, x. If g = f(q) then there is a linear transformation 
Tqf : TqQ — )• TgG whose matrix representation is simply the Jacobian matrix //(q). 
Working locally, we are free to choose coordinates so that the given configuration is the iden- 
tity e 6 G and so we are interested in T e G especially. This vector space represents infinitesimal 
displacements of the end-effector. It has additional structure, namely that of a Lie algebra, 
characteristically denoted $j: it has a bilinear, skew-symmetric "bracket" product \u\, Ui\ £ g 
for u\, «2 £ 0, that satisfies an additional property, the Jacobi identity. See, for example, Mur- 
ray et al. (1994); Selig (2005) for further details in the context of robot kinematics. The bracket 
provides a measure of the failure of a pair of displacements to commute. 

For the Euclidean group of displacements of a rigid body in R , its Lie algebra se(3) inherits 
the semi-direct product structure of SE(3) and is isomorphic to so (3) ffi t(3); the infinitesimal 
rotations so (3) can be represented by 3 x 3 skew-symmetric matrices 

(0 — W3 0J2 \ 
co 3 -wj (4) 

— u>2 to 1 / 

or equivalently the corresponding 3- vector w that spans the kernel of the matrix (0 for the zero 
matrix); the infinitesimal translations are also 3-vectors v. Hence elements X £ se(3), termed 
twists are represented by pairs of 3-vectors (u>, v). For X^O, the line in se(3) spanned by X 
is called a screw. 
The Lie bracket on sc(3) can be defined in terms of the standard vector product x on R by 

[(wi/Vi), (a>2> v 2)] = ( w l x W2, u>i X V2 + Vj X ujj) (5) 

Thought of as a 6-vector, the components of this representation are generally known as screw 
coordinates though more properly they are twist coordinates. The pitch of a twist (w, v) is 
the ratio of the two fundamental invariants, the Klein and Killing forms, expressed as scalar 
products of the component 3-vectors of the twists as: 

uj ■ v 
h = (6) 

A twist X of pitch corresponds to infinitesimal rotation about an axis and the corresponding 
screw can be identified with the line in R that is its axis; in that case, screw coordinates are 
identical with classical Pliicker line coordinates. When uj = 0, the pitch is not well-defined by 
this formula but is conventionally said to be 00. Note that the pitch is in fact an invariant of 
screws, not just twists. 
The Klein form arises as the quadratic form associated with the non-degenerate bilinear form 

Qo((wi,vi),(w 2 ,v 2 )) = 2(0*1 ■ v 2 + ^2- vj) (7) 

The form Qq gives rise to a natural pairing between the Lie algebra and its dual space of 
wrenches (force plus torque) so it is possible to identify wrenches and twists. As Qo is indefi- 
nite, there exist non-zero reciprocal twists Xj,X2 satisfying Qo(^i/-^2) = 0. In statics, a wrench 
X2 is reciprocal to a twist X\ if it performs no work on a body free to move along Xj . 
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In order to describe the infinitesimal capabilities of a rigid body with several degrees of free- 
dom, define a screw system to be any subspace S C se(3) (Davidson & Hunt, 2004; Gibson & 
Hunt, 1990). If X\, . . . , Xj- form a set of independent twists, then they span a fc-(screw) system. 
Associated with S is a reciprocal (6 — /c)-system S 1 - consisting of the constraints or wrenches 
that perform no work when acting on the end-effector. (These are not necessarily distinct from 
S.) 

4.3 Product of exponentials 

It is valuable to have a standard form in which to express the kinematic mapping. In particu- 
lar, the product of exponentials, allows us to make use of the rich theory of Lie groups. For any 
Lie group G there is a natural exponential mapping, exp, from the Lie algebra g to the group G 
itself. When the elements of G, and hence g, are written as matrices then for any matrix U £ g 

and ijeR, 

exp(qU) = £ — q" 6 G (8) 

n=0 "• 

The one-parameter subgroups (i.e. 1 -dimensional) of a Lie group G always have the form the 
form exp(qX), where X 7^ is an element of the Lie algebra g and i}6E. As noted previously, 
these correspond to the motions generated by R, P and H joints. Note that any non-zero mul- 
tiple of X may be used to represent the same joint: in effect, the joint is uniquely represented 
by a screw. For an R joint the pitch h = 0, while for a P joint h = 00. 

Brockett (1984) adapted an approach for representing kinematic mappings originally due to 
Denavit & Hartenberg (1955) and demonstrated that the motion of the end-effector of a serial 
manipulator can be written as a product of exponentials: 

f(qi,...,q k ) = exp(<frXi) • exp((j 2 X 2 ) • • • exp(q k X k ) (9) 

where 17, denotes the joint variable of the 2th joint and X; its twist relative to a fixed set of 
base link coordinates, for / = 1, . . . , k. An alternative approach uses coordinates in each 
link and expresses the invariant relation between successive links by a standard form of ma- 
trix in terms of its (Denavit-Hartenberg) design parameters. However the pure product of 
exponentials formulation permits the use of a classical formula from Lie theory, the Baker- 
Campbell-Hausdorff (BCH) formula (Donelan, 2007b; Selig, 2005). Given / in the form (9), 

since £ exp(^X) = X then at q\ = ■ ■ ■ = q k = 0, the twists X\, . . . , X; c span a screw 

system S of dimension S < k, that describes the infinitesimal capabilities of the end-effector. 
Here, 5 is the infinitesimal mobility. 

The product-of-exponentials formalism (9) can be extended to chains that include spherical 
joints. For any point a£l there is a 3-dimensional subgroup G a € S£(3) of rotations about 
a, leaving a fixed. If g a C se(3) is the corresponding Lie subalgebra then the restriction of 
the exponential on se(3) to g a is surjective so that every element of G a can be written (not 
uniquely) in the form exp(q\Xi + (72X2 + ^3X3) where X;, i = 1,2,3 form a basis for g a ; for 
example they can be taken to be infinitesimal rotations about three orthogonal lines through 
a. The product then includes an exponential of this extended form. 

5. Serial Manipulator Singularities 

The most important characteristic of a linear transformation, invariant under linear change 
of coordinates, is its rank, the dimension of its image. Since a linear transformation cannot 
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increase dimension and the image is itself a subspace of the range, the rank can be no greater 
than either the dimension of the domain or of the range of the transformation. This provides 
the basis for the precise definition of a singularity: 

Definition 1. A serial manipulator with kinematic mapping f : Q — »■ G, where the joint space Q has 
dimension k and the displacement group G has dimension n, has a kinematic singularity at q £ Qif 
rank Tnf has rank less than both k and n. 

In particular, if k = n then there is a kinematic singularity when rank Tqf < n, or equivalently 
det ]f(q) = 0. In terms of mobility, the definition is equivalent to S < u. 

Using topological methods, it can be shown that for standard architectures such as 6R serial 
manipulators, where the joint space is the 6-dimensional torus Q = T and the end-effector 
space is SE(3), the kinematic mapping must have singularities (Gottlieb, 1986). These can 
only be excluded from the the joint space by imposing restrictions on the joint variables. In 
particular, if the joint space is compact, such as T , then so its image, the work space, and 
is hence bounded in S£(3). The kinematic mapping will have singularities at the boundary 
configurations. However it may also have singularities interior to the workspace. 
A fundamental problem is to determine the locus of kinematic singularities in the joint space 
of a manipulator and, if possible, to stratify it in a natural way. As has been mentioned before, 
actually determining the locus remains a difficult problem for most manipulators. However 
one can address the question of whether the locus is itself a submanifold of the joint space 
using singularity theory methods. The singularities of rank deficiency 1 (corank 1) can be 
recognized by whether, at a given configuration, the first-order Taylor polynomial of the kine- 
matic mapping / : Q — > S£(3) lies in a certain manifold. Transversality to this manifold, a 
linear-algebraic condition that holds when a certain set of twists span the Lie algebra sc(3), is 
enough to guarantee that the corank 1 part of the singularity locus is a manifold of dimension 
1 6 — k\ + 1. The resulting condition involves the joint twists and certain Lie brackets involving 
them (Donelan, 2008). 

The approach can also, in principle, be extended to a manipulator architecture by including 
the design parameters. In this case, one may expect to encounter more degenerate singu- 
larities for specific values of the design parameter. An important problem is to determine 
the bifurcation set that separates classes within the architecture of manipulators with distinct 
topological type of singularity locus. 

6. Parallel Manipulator Singularities 

6.1 Infinitesimal analysis of closure 

In contrast to serial manipulators, in which all the joints are actuated, for a parallel manipu- 
lator only some of the joints will be actuated, the remainder being passive. The approach to 
parallel manipulator singularities pioneered by Gosselin & Angeles (1990) makes use of the 
actuated joint variables q and output (end-effector) variables x, constrained by an equation of 
the form: 

F(q,x) = (10) 

Differentiating (10), the infinitesimal kinematics can be written in the form: 
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Fig. 1. Planar 4R manipulator 



where the two Jacobian matrices L = 3F/3q, ] x = dF/dx depend on both q and x. If one 
assumes no redundancy, then the number of actuator variables equals the number of output 
variables, i.e. the mobility \i of the manipulator. The number of constraints, the dimension 
of the range of F , should then be the difference between the number of variables 2/< and the 
mobility u, hence also /(. Thus the Jacobians are both u x u matrices. 

This leads to three types of singularity, depending on loss of rank of L (type I), or J x (type II), 
or both (type III). A type I singularity is comparable to the kinematic singularity of a serial 
manipulator in Definition 1, where there exist theoretical end-effector velocities that are not 
achievable by means of any input joint variable velocity. On the other hand, in a type II 
singularity, there is a non-zero vector x in the kernel of ] x which therefore gives a solution 
to (11) with q = 0, in other words when the actuated joints are static or locked. Such a 
solution may be isolated but may also correspond to a finite branch of motion, referred to 
as an architecture singular motion (Ma & Angeles, 1992) or self-motion (Karger & Husty, 1998). 
(Note that this term is also used for motions of a serial redundant manipulator in which the 
end-effector remains static.) 

In practice, as noted by Gosselin & Angeles (1990), the constraint equations one actually for- 
mulates do not necessarily have the form (10). Denavit & Hartenberg (1955) observed that 
for a closed chain the matrix product must be the identity and, likewise, for the product-of- 
exponentials formulation. For a simple closed chain, the end-effector is identified with the 
base, so gives rise to a closure equation for the kinematic mapping of the form: 

<\>{qi,...,q k ) = exp^X^) ■ exp(q 2 X 2 ) ■ ■ ■ exp{q k X k ) = I (12) 

where I is the identity transformation that leaves the end-effector fixed with respect to the 
base link. 

By way of a relatively simple example, consider a planar 4R closed chain (Gibson & New- 
stead, 1986) where the closure equation involves the design parameters (link lengths) a, b, c, d 
and four joint variables qi, i = 1,2,3,4 as in Figure 1. Since the kinematics concern SE(2), 
which is 3-dimensional, there are three scalar closure equations, two for translation and one 
for rotation: 



acosqi + bcos(qi + q 2 ) + ccos(qi + q 2 + <fo) + dcos(q 1 + q 2 + q^ 
asinqi + bsin(qi + q 2 ) + csin(qi + q 2 + (J3) + dsin(^i + q 2 + qj, 

qi + q 2 + q: 



q 4 ) = (13) 

q 4 ) = (14) 

<7i + <?2 + % + <?4 = mod 27T (15) 
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Usually one joint variable is eliminated via (15), which is then omitted, and (13,14) simplified. 
One of cji, q$ is taken as actuator variable. Hence, to obtain a constraint of the form (10), it is 
necessary first to eliminate the remaining passive joint variables. This can be done by using 
cos 2 8j + sin 6, ■ = 1, i = 1, . . . , 4, at the cost of obtaining a formula involving the two branches 
of a quadratic. In particular, one loses differentiability where the discriminant vanishes. Al- 
ternatively, the equations can be rewritten as algebraic equations either using tan half-angle 
as variable, or replacing both cos 9j and sin 9,- by new variables, together with corresponding 
Pythagorean constraint equations. In either case it is, in theory, possible to use Grobner basis 
techniques to eliminate variables (Cox et al., 2004). 

The situation can, however, become much more complex. For the Gough-Stewart platform 
in its most general architecture, for a given set of actuator variables q there may be up to 40 
possible configurations x for the platform (Dietmaier, 1998; Lazard, 1993; Mourrain, 1993). 
The omission of passive joint variables has additional drawbacks. Firstly, the choice of actu- 
ated variable may be arbitrary, whereas it is sometimes preferable to allow freedom in this 
choice. Secondly, a manipulator may gain finite or infinitesimal freedom with respect to only 
its passive joints in certain configurations. This phenomenon was observed by Di Gregorio 
& Parenti-Castelli (2002) who showed that a 3-UPU manipulator designed for translational 
motion could undergo rotation in some configurations. Such solutions may not be apparent 
when solving the restricted constraint equations (10). 

The need to include passive joints for the instantaneous kinematics of a general parallel ma- 
nipulator was recognized by Zlatanov et al. (1994a;b). They distinguished six types of sin- 
gularity in total. In addition to those arising from inclusion of the passive joints, they also 
allowed that the instantaneous mobility could exceed the full-cycle mobility \i in (1). The re- 
sulting singularity type was denoted increased instantaneous mobility (IIM). The phenomenon 
had already been idenitified for simple closed chains by Hunt (1978), who termed it uncer- 
tainty configuration, as it corresponds to an intersection of branches of the configuration space, 
allowing the end-effector motion in at least two different modes. This has also been termed a 
configuration space singularity (Zlatanov et al., 2001) or topological singularity (Shvalb et al., 2009). 
Indeed, even in the relatively simple case of the planar 4R closed chain, if the link lengths 
e\ < e2 < £3 < £4 (in increasing order) satisfy the non-Grashof condition ej + £4 = £2 + e 3 
then the 4-bar can fold flat, with all joints collinear, and the configuration space has a singu- 
larity (Gibson & Newstead, 1986). 

6.2 A unified approach 

Consider a general parallel manipulator with / links and k joints, the /th joint having 5j degrees 
of freedom, i = 1, . . . , k. Suppose Q is the product of of the individual joint spaces, so that 
Q is a manifold of dimension d = YJj=\ $i- The configuration space C C Q is the set of joint 
variable vectors that satisfy the constraints imposed by the manipulator's structure. As noted 
in Section 3, the number of independent constraints is k — I + 1, the dimension of the cycle 
space of the graph. Let q\,...,q& denote the joint variables associated with twists X\, . . . , Xj. 
Each constraint can be written in the form ^>;(q) = L j = l,...,k—l + l with <pi as in (12). The 
map 

9 : Q ->■ SE(3) k -'+\ *(q) = (fr(q),...,fe-i+i(q)) (16) 

defines the configuration space as C = {q 6 Q : <t>(q) = (!,...,!)}. The Pre-Image Theorem 
of differential topology asserts that C is a manifold of dimension u = dim Q — dim S£(3) +1 
provided that Tq<t> has maximum rank for all q 6 C. Consider, for example, a Gough-Stewart 
platform having 6 UPS legs connecting the base to the platform, as in Figure 2. Each leg has 
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Fig. 2. 6 UPS parallel manipulator 



6 joint variables, hence d = 36, while the number of links, including the base is / = 14 and the 
number of joints k = 18. Given dimS£(3) = 6, it follows that the Jacobian is 30 x 36. Although 
this is rather large, we can determine its structure quite simply. Suppose the legs are numbered 
r = 1, . . . , 6 and the twists in each leg denoted X rs , r,s = 1, . . . , 6. Five independent closure 
equations arise from the closed chains consisting of leg 1, leg r, the base and the platform, for 
r = 2, ... ,6 as follows: 

exp(q n X n ) exp{q 12 X 12 ) exp(q 13 X 13 ) exp(q 1A X u + q i5 X 15 + q 16 X 16 ) 

■ exp(-£j r4 X r4 - (? r5 X r5 - q r6 X r6 ) exp(-q r3 X r3 ) exp{-q r2 X r2 ) exp(-q rl X n ) = I (17) 

The Jacobian matrix consists of 5 rows of 36 twists (6-vectors), the jth row having in the col- 
umn of the zth joint variable either the corresponding twist X,, if that joint is involved in the 
loop described by the closure equation for tpj, or else 0; for example the first row is 

(Xji X 12 ■ ■ ■ X 16 X 2 i X 22 ■ ■ ■ X 26 ■ ■ ■ 0) (18) 

It follows that a necessary and sufficient condition for maximum rank is that the pairwise 
vector sum of the screw systems of legs span se(3). Alternatively, there is a singularity if there 
exists a common reciprocal wrench. This is a configuration space singularity. 
Suppose now that the the Jacobian is of full rank, so that the configuration space is a manifold 
whose dimension is the full-cycle mobility \i in the CGK formula (1), or simply that this is true 
in a neighbourhood of a configuration. Can the full mobility of the platform be achieved using 
a given set of u actuated joints? This can be answered using the Implicit Function Theorem. 
One requires the corresponding joint variables to give local coordinates for C. The theorem 
asserts that this is the case if the square matrix, obtained by deleting the }i columns of the 
Jacobian corresponding to the actuator variables, is non-singular. If that fails to be the case at 
some configuration q 6 C, then there is a direction in the tangent space TqC that projects to 
in the actuator velocity space but which corresponds to a non-zero velocity of the platform. 
This is therefore a type II singularity in the nomenclature above, or what might be termed an 
actuator singularity. 

Finally, the type I or kinematic singularities for a parallel manipulator at a non-singular point 
of the configuration space are simply the singularities of the forward kinematics from C to 
S£(3). 
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Even though this model gives a reasonably complete picture of the singularity types possible 
for a parallel manipulator, the paradoxical mechanisms such as Bennett, Bricard, Goldberg 
etc., are not explained; they correspond here to manipulators for which every configuration is 
a configuration space singularity, as the constraint Jacobian rank is non-maximal. Neverthe- 
less, their configuration spaces are manifolds, but of the "wrong" dimension. 

7. Outlook 

The analysis of manipulator singularities is a burgeoning area of research. This chapter has 
touched briefly on some key themes. There are significant open problems, including under- 
standing over-constrained mechanisms, genericity theorems for manipulator architectures, 
higher-order analysis and topology of the singularity loci, singularities of compliant mech- 
anisms, bifurcation analysis and many more. The practical problems of manipulator design 
and control remain central goals, but their resolution involves many branches of mathemat- 
ics including algebraic geometry, differential topology, Lie group theory, geometric algebra, 
analysis, numerical methods and combinatorics. 
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1. Introduction 

Since their appearance in the early 1960's, industrial robots have gained wide popularity as 
essential components in the construction of automated systems. Reduction of manufacturing 
costs, increase of productivity improvement of product quality standards, and the possibility 
of eliminating harmful or repetitive tasks for human operators represent the main factors 
that have determined the spread of the robotics technology in the manufacturing industry. 
Industrial robots are suitable for applications where high precision, repeatability and tracking 
accuracy are required. These facts give a great importance to the analysis of the actual control 
schemes of industrial robots (Camarillo et al., 2008). 

It is common to specify the robotic tasks in terms of the pose (position and orientation) of 
the robot's end-effector. In this sense, the operational space, introduced by O. Khatib (1987), 
considers the description of the end-effector's pose by means of a position vector, given in 
Cartesian coordinates, and an orientation vector, specified in terms of Euler angles. On the 
other hand, the motion of the robot is produced by control signals applied directly to the joint 
actuators; further, the robot configuration is usually measured through sensors located in the 
joints. These facts lead to consider two general control schemes: 

• The joint-space control requires the use of inverse kinematics to convert the pose desired 
task to joint coordinates, and then a typical position controller using the joint feedback 
signals is employed. 

• The operational-space control uses direct kinematics to transform the measured positions 
and velocities to operational coordinates, so that the control errors are directly com- 
puted in this space. 

The analysis of joint-space controllers is simpler than that of operational-space controllers. 
However, the difficulty of computing the inverse kinematics, especially for robots with many 
degrees of freedom, is a disadvantage for the implementation of joint-space controllers in 
real-time applications. 



418 Advances in Robot Manipulators 

Most of industrial robots are driven by brushless DC (BLDC) servoactuators. Typically, this 
kind of servos include electronic drives which have internal joint velocity and torque con- 
trollers (Campa et al., 2008). Thus, the drive's input signal can be either the desired joint 
velocity or torque, defining the so-called velocity or torque operation modes, respectively. In 
practice, these drive inner controllers are fixed (they are usually PI controllers tuned with a 
high proportional and integral gains), and an overall outer loop is necessary to achieve the 
control goal in operational space. 

Aicardi et al. (1995) were the first in analyzing such a hierarchical structure to solve the 
problem of pose control, but considering an ad-hoc velocity inner loop. Kelly & Moreno 
(2005) used the same inner controller, together with the so-called resolved motion rate controller 
(RMRC) proposed by Whitney (1969), to solve the problem of operational space control. More 
recently, Camarillo et al. (2008) invoked some results from (Qu & Dorsey, 1991) to the analy- 
sis of a two-loop hierarchical controller using the RMRC as the outer loop and the typical PI 
velocity controller in the inner loop. 

The Mitsubishi PA-10 arm is a general-purpose robotic manipulator with an open architec- 
ture, which makes it a suitable choice for both industrial and research applications (Oonishi, 
1999). The PA-10 system has a hierarchical structure with several control levels and standard 
interfaces among them; the lowermost level is at the robot joint BLDC servoactuators, which 
can be configured either in velocity or torque mode; however, the original setup provided by 
the manufacturer allows only the operation in velocity mode. 

The aim of this work is twofold. Firstly, we review the theoretical results that have been 
recently proposed in (Camarillo et al., 2008), regarding the stability of the operational space 
control of industrial robots, assuming inner joint velocity PI controllers, plus the RMRC as 
the outer loop. Secondly, as a practical contribution of this chapter, we describe the steps 
required to configure the PA-10 robot arm in torque mode, and include some experimental 
results obtained from the real-time implementation of two-loop operational space controllers 
in a PA-10 robot arm which is in our laboratory. 

After stating the concerns of the chapter, and recalling the main aspects of modeling and 
controlling industrial robots (sections 1 and 2) we review the stability analysis of the two- 
loop operational-space motion-control scheme in Section 3. The description of the PA-10 
open-architecture is given in Section 4, while Section 5 explains the hardware and software 
interfaces that were developed in order to make the PA-10 work in torque-mode. To show the 
feasibility of the operational-space control scheme in Section 3, we carried out some real-time 
experiments using the platform described before in a real PA-10 robot; this is explained in 
Section 6. Finally, concluding remarks are set in Section 7. 

2. Modeling and Control of Industrial Robots 

2.1 Kinematic and Dynamic Modeling 

From a mechanical point of view, industrial robots are considered to have an open kinematic 
chain, conformed by rigid links and actuated joints. One end of the chain (the base) is fixed, 
while the other (usually called the end-effector) is supposed to have a tool or device for execut- 
ing the task assigned to the robot. As this structure resembles a human arm, industrial robots 
are also known as robot manipulators. 

2.1.1 Kinematics 

In a typical industrial robot the number of degrees of freedom of the robot equals the number 
of joints in the kinematic chain; let n be that number. The configuration of the robot can then 
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be described by a set of n joint coordinates: 

q= [<?i qi ■■■ q n ] T e R". 

On the other hand, let m be the number of degrees of freedom required to specify the task to be 
executed by the robot. Notice that, in order to ensure the accomplishment of the desired task, 
then n > m. If n > m we have a redundant manipulator, which has more degrees of freedom 
that those required to execute the given task. 

It is common that the task is specified in terms of the pose (position and orientation) of the 
robot's end-effector (usually time-varying). In the general 3D-motion case, we require three 
degrees of freedom for the position and other three for the orientation, thus m = 6. Let x be 
the set of operational coordinates for describing the pose of the end-effector, then 

x=[x l x 2 ■■■ x 6 ] T e R 6 . 
The direct kinematic model of the robot can be written as 

x = h(q), (1) 

where h(q) : R" — > R is a function describing the relation between the joint coordinates and 
the operational coordinates. Furthermore, the differential kinematics equation can be obtained 
as the time derivative of the direct kinematics equation (1), i.e. (Sciavicco & Siciliano, 2000): 

* = /(*)*, (2) 

where J(q) 6 R 6x " is the so-called analytic Jacobian matrix. 

To obtain the inverse differential kinematics, we use the right pseudoinverse of J(q), which is 

given by (Sciavicco & Siciliano, 2000): 

}(c,)' = J(q) T [l(q)J(q) T Y\ (3) 

in such a way that ]{q)]{q)^ = I, being I G R the identity matrix. Notice that if n = 6 then 

}(q) reduces to the conventional inverse matrix ]{q) . 

In the general case, the inverse differential kinematics is given by 

q = ](q)'x+[l-J{q)'](q)] qo G R ffl (4) 

where the first term is known as the minimal norm solution of (2), and matrix [l — J(q) J{q)] 
is the orthogonal projector to the null space of }(q), AA(/), defined as 

7V(/) = {we~R" :}w = 0}. 

Any vector belonging to A^(/) has no effect in the motion of the robot's end-effector, but in 
the self-motion of its joints (Nakamura, 1991). And as the second term of (4) always belongs 
to N(J), independently of the vector q (that can be easily shown by left multiplying (4) 
by ](q)), then q can be considered as a joint velocity vector chosen in a way that allows 
performing a secondary task, without affecting the motion of the robot's end-effector. 
There exist different approaches for choosing vector q , most of them considering that it is the 
gradient of a suitable cost function. See (Sciavicco & Siciliano, 2000) for more information on 
this. 
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In the analysis forthcoming, it is assumed that the robot's end-effector motion makes J(q) to 
be full rank during the whole task. Also, it is assumed that the following inequalities stand, 
for some positive constants kj, kj„, and k^j„: 



whir 



\\J(q)\\<kj V^GR", 
H/(<7) + ll < k Jp , V,6K», 

s{«"'} 



<k dJp , V ? 6E". 



(5) 
(6) 

(7) 



2.1.2 Dynamics 

The dynamic model of a serial n-joint robot free of friction is written (Kelly et al., 2005): 

M(q)q + C(q,q)q+g(q) = r, 



(8) 



where q G R" is the vector of joint coordinates, q 6 R™ is the vector of joint velocities, q G R" 

is the vector of joint accelerations, T G R" is the vector of applied torque inputs, M(q) G R" x " 

is the symmetric positive definite manipulator inertia matrix, C(q,q) G R" x " is the matrix of 

centripetal and Coriolis torques, and g(q) G R" is the vector of gravitational torques. 

Some useful properties of the dynamic model (8) that will be used in this document, are the 

following (Kelly et al., 2005): 

Property 1: C(q, q) can be chosen so that matrix jM(q) — C(q, q) is skew-symmetric, i.e., 



-M(q)-C(q,q) 



y = 0, Vi/GR". 



(9) 



Property 2: There exist positive constants fc^j, fcrj an d kg such that, for all u,w,y G R", we 
have 



\\M(u)y\\ < k M \\y\\, 
\C(u,w)y\\ < kc\\iv\\\\y\\ 
k(w)ll < h. 



(10) 
(11) 
(12) 



2.2 Hierarchical control 

It is well-known that most of industrial robots have internal joint velocity PI controllers, which 
are usually tuned with very high proportional and integral gains. In practice, these internal 
controllers are fixed (they belong to each of the joint actuator's servodrives), and an outer 
loop is necessary to achieve the control goal in operational space. This two-loop hierarchical 
structure is shown in Figure 1. 



Kinematic 
Control 



Joint 
Velocity 
Control 



Robot 



Direct 
Kinematics 



Analytic 
Jacobian 



Fig. 1. Typical scheme of a hierarchical structure for operational space control. 
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2.2.1 Kinematic controller 

By kinematic control we refer to any scheme that uses an inverse Jacobian algorithm to re- 
solve the desired joint velocities directly from the pose variables of the desired task. Thus, a 
kinematic controller is used as the outer loop in the hierarchical controller of Figure 1, which 
provides the desired joint velocities for the inner velocity controller. 

In this chapter we use as kinematic controller the so-called resolved motion rate control 
(RMRC), which was first proposed by (Whitney, 1969). Using this scheme, the desired joint 
velocity for the inner loop, can be written as 

v d = J(q)' [*„ + Kx] +[l- J(q) f J(q)] q , (13) 

where vj G R" is the desired joint velocity vector, xj G R is the time derivative of the desired 
pose vector x& in operational space, x = x& — x E R is the pose error vector in operational 
space, and K G R is a symmetric positive definite matrix of control gains. The second term 
in (13) is added to include the possibility of controlling a secondary task in redundant robots. 

2.2.2 Joint velocity PI controller 

For the inner loop in Figure 1, we consider the classical joint velocity proportional integral PI 
controller, commonly used in industrial robots, which can be written as 

T = K p v + K£, (14) 

t = v, (15) 

where 

= Vd - q € R", (16) 

£ = L v(cr) da, and K„,Kj G R" x " are diagonal positive definite matrices. 



Without loss of generality, let us take 

K p = [k p + y]l, (17) 

K t = xK p , (18) 

where k„, y and a are strictly positive constants. Notice that in such case (18) can be written 
as 

K { = [k t + « 7 ] I, (19) 

(20) 



with kj = ockp, or 



1<n 



3. Stability Analysis 

The stability analysis presented here is taken from (Camarillo et al., 2008). More detail can be 
found in that reference. 

First, some remarks on notation: We use An^lA} and A max {A} to represent, respectively, 
the smallest and the largest eigenvalues of a symmetric positive definite matrix A(y), for any 
y G R". Given y G R" and a matrix A (y) the Euclidean norm of y is defined as \\y\\ = \/y T y, 
and the induced norm of A(y) is defined as || A(y) \\ = \/ h max {A T A} . 



422 



Advances in Robot Manipulators 



Note in Figure 1 that the closed-loop system is formed by the RMRC controller (13), the joint 
velocity PI controller (14)-(15), and the robot model, given by (8), (1) and (2). 
Left multiplying (13) by J(q), using (2), and (16), we get 

x = -Kx + J(q)v. (21) 

Substituting (14) into the robot dynamics (8) we have 

M(q)ij + C(q, q)q + g(q) = K p v + K£. (22) 

Now, taking into account (16) and its derivative, we get 

M{q)S + C{q,q)v + K p v + K£ = M{q)v d + C(q,q)v d + g(q). 

We can add the terms (x.M{q)v + ocC(q, <j)£ to both sides of the last equation, to obtain 

M(q) [v + ocv] + C{q, q) [v + a$\ + K p v + K;£ = 

M(q) [v d + OCV] + C(q, q) [v d + a!,] + g(q). (23) 

Finally, from (21), (15) and (23), we get the closed-loop system in terms of the state vector 



GR' 



6+2n. 



d 


X 




Jt 


V 





J(q)v-Kx 



clv - M(q)- 1 [C{q, q) [v + a£] + K p v + Kg - d(t, z) 



(24) 



where the term 



d(t, z) = M(q) [v d + CCV] + C(q, q) [v d + «£] + g(q) (25) 

is considered as a disturbance and, as it is shown in (Camarillo et al., 2008), is upper bounded 
for all t > t o by a quadratic polynomial: 

||d(f,z)||< e2 ||z(t)|| 2 + c 1 ||z(f)||+eo, 



(26) 



where Co, c,\, c,i are positive constants, assuming that || v d \\ and || v d \\ are also bounded. 
Substituting the definitions (17) and (19) in (24), we get 



dt 



}(q)v-Kx 

V 

-av-M{q)- 1 [C(q,q) [v + a£\ + k p [v + a?] + 7 [v + a£ ] -d(t,z)]_ 



(27) 



In accordance with the theory of perturbed systems (Khalil, 2005), we define 



dt 



J(q)v-Kx 

v 
-av - M(q)- 1 [C(q, q) [v + cd£\ + k p [v + aft] 



(28) 



as the nominal system from the closed-loop system (27). As we can see, (28) is free of distur- 
bances, and the origin of the state space z = is an equilibrium of (28). 

In order to show the stability of the overall closed-loop system (27), we use the methodology 
proposed in (Khalil, 2005), that is: 

(a) First, to prove the asymptotic stability of the nominal system (28). 

(b) Then, to prove the uniform ultimate boundedness of the overall closed-loop system 
(27). 
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3.1 Stability of the nominal system 

To prove that the origin of the nominal system (28) is exponentially stable, we propose the 
following Lyapunov function candidate, inspired from (Qu & Dorsey, 1991): 



V(t,z) = -x T x + af 



k p I+ -ocM(q) 



It is easy to show that 



T 
Pi 



x\\ 

u\\ 

\\v\ 



< V(t,z) < 



Z + <xZ T M{q)v+-v T M(q)v. 



-\T rn-in 



Pi 



\x\ 

k\\ 

\\v\\ 



with 



Pi 



Pi 



\ 

ak p + i« 2 A min {M} - 2 aA max {M} 

-iaAmaxjM} jAminjM} 



\ 

ak v + la 2 \max{M} iaA max {M} 
2«A max {M} 2A max {M} . 



so that the Lyapunov function candidate (29) satisfies 

AiUzll 2 < V{t,z) < A 2 ||z|| 2 , 
for some positive constants X\, A 2 , given by 

M = A m in{fl}, A 2 = A max {P 2 }. 
Notice that V(t,z) is a globally positive definite function if 



or, using (20), if 



a < 



ki < 



2/c p A min {M} 
ALxiMj-A^jM}' 

2/c 2 p A min {M} 



(29) 



(30) 



(31) 



(32) 



A?nax{M} - A^jM} 
On the other hand, the time derivative of the Lyapunov function candidate (29) is given by 

1 



(33) 



V(t,z) = x 1 x + ak p $ 1 Z + l x z Z 1 M(q)Z+- l x z S , M(q)$ + KZ M(q)v + cc? M(q)v 



1 



(34) 



+a£ J M(q)v + v 1 M(q)v + -v' M(q)v, 
which along the solutions of the nominal system (28) results in 

V(t, z) = -x T Kx - otkift - k p v T v + x T ](q)v, 

where we have used the definition of a in (20), and the skew-symmetry property (9) to elimi- 
nate some terms. 
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Then, under the assumption that the Jacobian matrix J(q) is bounded, see (5), it is easy to 
show that 



with 



V(t,z) < - 



Q 



11*11 


T 


11*11 


llffll 


Q 


H\\ 


||0|| 




\\v\\ 



min i & i 







-W 




-\kj 





kp 



So that (35) can be written as 



where 



Notice that we can choose 



V(t,z) < -A 3 ||z|| 2 , 



A3 = A min {Q}. 



AminW > 



4k/ 



(35) 

(36) 

(37) 
(38) 
(39) 



so that Q is positive definite and V(t,z) is globally negative definite. 

Thus, provided that (33) and (39) are satisfied, and according to Lyapunov's direct method 
(Khalil, 2005), we conclude the exponential stability of the origin of the nominal system (28). 
This implies that the origin of the state space is attractive, i.e. 



lim z(t) 

t->co 



lim 

t— >oo 



v(t) 



3.2 Stability of the overall closed-loop system 

We have shown in the previous subsection that the equilibrium of the nominal system (28) 
is exponentially stable by choosing the feedback gains A:,, and A m ; n {i<C} according to (33) and 
(39), respectively. Now, we are able to show that, by properly choosing the feedback gain 7 
of the joint velocity PI controller, the solutions z(t) of the overall closed-loop system (27) are 
uniformly ultimately bounded. 

First, we recall the following fact (Qu & Dorsey, 1991), that will be useful for our purposes. 
Fact 1: Let 

g(\\z\\) = -a 1 ||z|| 2 + a 2 ||z||+a3, (40) 

be a quadratic polynomial with ct\ > 0, and oci, 0C3 > 0. Given 



a 2 + Joel + 4«ia3 



2« 1 



(41) 



then 



g(l) = 0, 
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• g"(||z||) < 0, and strictly decreasing, for all \\z\\ > n. 



Now, we recall the following theorem in (Camarillo et al., 2008), which is a variation of Theo- 
rem 4.18 in (Khalil, 2005). 
Theorem 1: Let 

z = f(t,z), (42) 

be a system that describes a first-order vector differential equation, where f : [0, oo) x D — > R m 
and D C R m , is a domain that contains the origin. Let V(t,z), with V : [0, oo) x D — > R, be a 
Lyapunov-like function that satisfies 

Aillzf < V{t,z) < A 2 ||z|| 2 , (43) 

V{t,z) < g{\\z\\) < 0, M\\z\\>n, (44) 

for all t > to, and for all z £ D, with K\,Xi > 0, g(||z||) and r\ defined in Fact 1 by (40) and 
(41), respectively. Then, there exists T > (dependent on z(fo) an d n) such that the solution of (4:2), 
satisfies the following properties, for any initial state z$ = z(tg): 

A. Uniform boundedness 

||z(f)|| < J^max{||z ||,J/}, V f > i - (45) 



B. Uniform ultimate boundedness 



\z(t)\\< J^rf, Vt>t + T, (46) 



for all n 1 > r\. 



V 
The proof of Theorem 1 can be found in (Camarillo et al., 2008). Theorem 1 is the base for the 
following result: 
Proposition 1: Consider the overall closed-loop system (27) with the outer-loop kinematic controller 

v d = J(q) f [x d + Kx], 

and the inner-loop velocity controller 

T = K p v + K£ = [k p + 7 ]z5 + [k { + 7a]?. (47) 

Suppose the control gains are chosen so that kp, kj and K satisfy (33) and (39). Moreover, y is a positive 
scalar such that 

7 > - [o- 2 G2 + cr ei + Co ] , (48) 

where c,q, c,\ and c,2 are the coefficients of the upper bound of \\ d(t, z) ||, given in (26), e is a positive 
constant such that 

< e < -?-, (49) 

52 
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and 



with 



^max{|| Z (f )||,7}, (50) 



egl + V e 2 Si + 4eg [A3 - eg 2 ] 

« = -^M^A ' (51> 

where K\, A2, are defined in (31), and A3 is in (38). 

Then, the system is stabilizable in the sense that exists T > such that the solution z(t), with initial 

state zq = z(frj) satisfies 

||z(f)|| < o~, Vf>fo (uniform boundedness), (52) 

ll z WII — tlTf'' V f > fo + T (uniform ultimate boundedness). (53) 

V ! 

for all ij' > ij, where (52) and (53) regard to the uniform and uniform ultimate boundedness, respec- 
tively. 

V 
The proof of Proposition 1 is also in (Camarillo et aL, 2008); it employs (25) and Theorem 1. 
The following remarks can be set from this result: 

Remark 1. Notice that ifkp^>co in the PI controller then, from (20), a. — > and, considering (36), 
and (49), we get A3 = A m i n {Q} — ¥ and e — > 0. Moreover, from (51), e — ¥ implies that fj — > 0, 

the uniform bound a becomes a = \ ^\\z{to)\\, and the system tends to be exponentially stable. 



Remark 2. 


It is noteworthy 


that the joint velocity PI 


controller 






t = l k p + i\v 


+ [ki + 7*]{ 






i = v 




can be also written as 










T = [k p 
1 


+ 7]z5 + ay 






k p + 7 V " °- 





Thus, if 7 — > 00, f/ie«, i; — >• 0, and 4 — ^ ^d- ^.'so, considering (17) and (19), 7 — >• 00 implies 
Kp,Kj —)• 00. 

From the previous remarks we can conclude that the common assumption of having 
high-value gains in the inner velocity PI controller of industrial robots leads to small tracking 
errors during the execution of a motion control task. 



4. The PA10 Robot System 

The Mitsubishi PA10 arm is an industrial robot manipulator which completely changes the 
vision of conventional industrial robots. Its name is an acronym of Portable General-Purpose 
Intelligent Arm. There exist two versions (Higuchi et aL, 2003): the PA10-6C and the PA10- 
7C, where the suffix digit indicates the number of degrees of freedom of the arm. This work 
focuses on the study of the PA10-7CE model, which is the enhanced version of the PA10-7C. 
The PA10 arm is an open architecture robot; it means that it possesses (Oonishi, 1999): 
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• A hierarchical structure with several control levels. 

• Communication between levels, via standard interfaces. 

• An open general-purpose interface in the higher level. 

This scheme allows the user to focus on the programming of the tasks at the PA10 system's 
higher level, without regarding on the operation of the lower levels. The programming can 
be performed using a high-level language, such as Visual BASIC or Visual C++, from a PC 
with Windows operating system. The PA10 robot is currently the open-architecture robot 
more employed for research (see, e.g., Kennedy & Desai (2003), Jamisola et al. (2004), Pholsiri 
(2004)). 

4.1 Basic structure of the PA10 robot 

The PA10 system is composed of four sections or levels, which conform a hierarchical structure 
(Mitsubishi, 2002a): 

Level 4: Operation control section (OCS); formed by the PC and the teaching pendant. 

Level 3: Motion control section (MCS); formed by the motion control and optical boards. 

Level 2: Servo drives. 

Level 1: Robot manipulator. 

Figure 2 shows the relation existing among each of the mentioned components. The following 
subsections give a more detailed description of them. 



Arm Mechanism 

Level 1 



Operation Control 

Level 4 




Fig. 2. Components of the PA10 system 
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4.1.1 Operation control section 

This section is the higher level in the PA10 system's hierarchy; it receives its name because it 
is in this section in which the operator programs the tasks to be performed by the robot. This 
can be done by a control computer or a teaching pendant. The control computer is a PC with 
an MS-Windows operating system. For the programming of the robot tasks, the operator can 
employ the programs provided by the manufacturer, or he can develop his own applications 
using Visual C++ or Visual BASIC and some API functions from the so-called PA library. 
It is through this functions that the user can select the control scheme he wants for the robot, 
being the most common the joint velocity control, the joint position control and the pose con- 
trol. More information on how these schemes can be implemented, as well as an experimental 
comparison among them, can be found in (Camarillo et al., 2006). It is worth mentioning, 
however, that all of these schemes work with the servo-drives configured in velocity mode. 
Independently on the way the tasks to be performed are specified in the operation control 
section, the information which is passed to the next level is always the same in this basic con- 
figuration. This allows to keep a hierarchical structure, but at the same time, it sets limitations 
to the versatility of the whole system. 

For the experimental evaluation shown in Section 6, we decided to use the control computer 
and a real-time application, developed in Visual C++. See section 5.3 for a description of this. 

4.1.2 Motion control section 

It is in this section where the joint velocity commands for the servo-drives (Level 2) are com- 
puted, using the information programmed by the user in Level 4. To do so, the original PA10 
system uses the MHI-D7281 motion control board, from Mitsubishi Heavy Industries, which is 
plugged in the PCI bus of the control computer. It is in this board where, among other things, 
the joint position and pose kinematic controllers, together with the interpolation algorithms 
for generating smooth trajectories, are implemented. 

Communication among the control computer and the driver cabinet is done via optical fibers, 
using the ARCNET protocol. The motion control board counts on a converter to encode sig- 
nals to the ARCNET format. More information on this can be found in Section 5.1. 
A board model MHI-D7210 is dedicated to convert the electrical signals from the motion con- 
trol board into optical signals. This board is called the optical board in Figure 2. For more 
information on the motion control board, and the optical board, see (Mitsubishi, 2002a). 

4.1.3 Servo-drives 

The seven drives of the PA10 servomotors are contained in a cabinet (driver) which connects 
to the optical-interface board via optical fiber. In addition, two cables connect the cabinet to 
the robot; one contains the power signals for the servomotors, while the other transmits the 
feedback signals from the joint position sensors. 

PAlO's servomotors are brushless DC type, and they are coupled to the links via harmonic 
drives. The servo-drives can be configured in torque or velocity modes. However, if the 
motion control board in the MCS is used, then it is only possible to configure the drives in 
velocity mode. In order to use the PA10 robot in torque mode, it is necessary to disable the 
MHI-D7281 board, and replace it by one which allows the operator to have direct access to the 
drive signals from the PC. This is explained in Section 5.2. 

In velocity mode, the drives include an inner velocity loop which, according to the docu- 
mentation provided by Mitsubishi (Mitsubishi, 2002b) is a digital PI controller with a 400 ^s 
sampling period. Drives also include a current (torque) PI controller, with a 100 /(s sampling 
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period, which, as indicated in (Campa et al., 2008) has an insignificant effect in the servomo- 
tor's dynamics at low velocities. 

4.1.4 Robot manipulator 

The PA10 robot is a 7-dof redundant manipulator with revolute joints. Figure 3 shows a 
diagram of the PA10 arm, indicating the positive rotation direction and the respective names 
of each of the joints. 



Axis 7 (W2) 
Axis 5 (E2) 




Axis 1 (SI) 



Fig. 3. Mitsubishi PA10-7CE robot 



4.2 Modeling of the PA10 arm 

As the PA10-7CE is a robot with seven degrees of freedom, then n = 7, and the direct kine- 
matics function / : 1R — ¥ R has the form x = /(<?). 

In this work, we consider the vector of operational coordinates x is formed by three Cartesian 
coordinates (x,y,z) and three Euler angles (a,/3,7), given by the ZYX convention (Craig, 2004). 
Using the traditional methodology, based on the Denavit-Hartenberg convention (Denavit & 
Hartenberg, 1955), we obtain for the PA10 arm the following expressions: 
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[-([(c a c 2 c 3 - S!S 3 )c 4 - C!S2S4]c 5 - [-(-c a c 2 s 3 - S!C 3 )]S 5 )S 6 
+ (-[cic 2 c 3 - S!S 3 ]s 4 - C!S2C 4 )c 6 ] d 7 

+ [-(C1C2C3 - S!S 3 )s 4 - C!S2C 4 ] d 5 - C 1 S 2 d 3 , 

[-([(S1C 2 C 3 + CJS 3 )C 4 - S!S 2 S 4 ]C 5 - (-(-SlC 2 S 3 + C!C3))S 5 )S 6 

+ (-[sic 2 c 3 + cis 3 ]s 4 - sis 2 c 4 )c 6 ] d 7 

+ [- (sic 2 c 3 + cis 3 )s 4 - sis 2 c 4 ] d 5 - sis 2 d 3/ 

[-([s 2 c 3 c 4 + c 2 s 4 ]c 5 - s 2 s 3 s 5 )s 6 + (-s 2 c 3 s 4 + c 2 c 4 )c 6 ] d 7 

+ [-s 2 c 3 s 4 + c 2 c 4 ] d 5 + c 2 d 3 , 

atan2 (b,a) , 

a 



R = atan2 -c, 

V cos(a) 

7 = atan2 (d, e) . 

where the terms c, y s, correspond to cos(cji) y sen(^,), respectively and d,- is the length of the 
f-th link, with i = 1,2, ... ,7. The required values for this robot are d\ = 0.317 m, d 3 = 0.450 
m, £?5 = 0.480 m, and dj = 0.070 m. For computing the Euler angles it is employed the inverse 
tangent function with two arguments (atan2) and the following expressions: 

« = [([(c 1 c 2 c 3 -sis 3 )c 4 -cis 2 s 4 ]c 5 + [-C!C 2 S 3 -S1C3]S 5 )C 6 
-(-[-(cjc 2 c 3 - sis 3 )s 4 - cis 2 c 4 ])s 6 ] c 7 
+ [-([cic 2 c 3 - sis 3 ]c 4 - cis 2 s 4 )s 5 + (-C!C 2 S 3 - sic 3 )c 5 ] s 7 

b = [([(S1C 2 C 3 + C!S 3 )C 4 - S!S 2 S 4 ]C 5 + [-S!C 2 S 3 + C!C 3 ]S 5 )C 6 
-(-(-(S!C 2 C3 +C!S 3 )S 4 - S!S 2 C 4 ))S 6 ] c 7 
+ [-([S1C 2 C 3 + C!S 3 ]C 4 - S!S 2 S 4 )S 5 + (-S!C 2 S 3 + C!C 3 )C 5 ] s 7 

c = [([-s 2 c 3 c 4 -c 2 s 4 ]c5 + s 2 s 3 S5)c 6 - (-[s 2 c 3 s 4 -c 2 c 4 ])s 6 ]c 7 

+ [- (-s 2 c 3 c 4 - c 2 s 4 )s 5 + s 2 s 3 c 5 ]s 7 
d = - [([-s 2 c 3 c 4 - c 2 s 4 ]c 5 + s 2 s 3 s 5 )c 6 - (-[s 2 c 3 s 4 - c 2 c 4 ])s 6 ] s 7 

+ [- (-s 2 c 3 c 4 - c 2 s 4 )s 5 + s 2 s 3 c 5 ]c 7 
e = - [-([-s 2 c 3 c 4 -c 2 s 4 ]c 5 +s 2 s 3 s 5 )s 6 - (-[s 2 c 3 s 4 -c 2 c 4 ])c 6 ] 

By reasons of space, we do not include here the analytic Jacobian of the PA10 robot; however, 
it is possible to obtain it from the direct kinematics, since 

Other way of computing the analytic Jacobian is via the geometric Jacobian, Jo(q), which re- 
lates the joint velocities with the linear and angular velocities, v and w, respectively, according 
to (Sciavicco & Siciliano, 2000) 



Motion Control of Industrial Robots in Operational 

Space: Analysis and Experiments with the PA10 Arm 431 



After computing the geometric Jacobian, using standard algorithms (Sciavicco & Siciliano, 
2000), we can use the following expression: 



m 



I 

T(a,j6,7) 



Jg(i) 



where T(a, /?, 7) is a representation transformation matrix which depends on the Euler angle 
convention employed. In the case of the ZYX convention, we have that 



T(a,/3, 7 ) 



— sin(a) cos(a) cos(/3) 

cos(a) sin(a) cos(/3) 

1 -sin(j8) 



Regarding the dynamic model of the PA10-7CE robot, it was not required for the experiments 
described in Section 6, although it can be found in (Ramirez, 2008). 

5. Real-time Control Platform 

In this section we first describe the general characteristics of the ARCNET protocol, and give 
the required specifications for its use in the PA10 system. After that, we describe the modifi- 
cations to the hardware, and the software application we have developed for controlling the 
PA10 robot in real-time. 

5.1 The ARCNET protocol 

ARCNET (acronym of Attached Resource Computer Network) is a protocol for data trans- 
mission in a local area network (LAN). It was developed by Datapoint Corporation and it 
became very popular in the 80's. In the current days, even though it has been relegated by 
Ethernet in commercial and academic networks, it is still used in the industrial field, since its 
deterministic behavior and robustness are appropriate for the control of devices. 
ARCNET handles data packets of variable length, from to 507 bytes, and operates at veloci- 
ties from 2.5 Mbps to 10 Mbps, so that it is possible to have a fast response for short messages. 
Another advantage is that it allows different types of transmission media, being the twisted 
cable, the coaxial cable and the optical fiber the most common. Moreover, the data interchange 
is implemented by hardware, in an ARCNET controller IC, so that the basic functions such as 
error checking, flux control and network configuration are performed directly in the IC. 
Each device connected to an ARCNET network is known as a node, and it must contain a 
controller IC as well as an adapter for the type of cable employed. A MAC address identifier 
is assigned to each node. An ARCNET network can have up to 255 nodes with a unique 
identifier (or node number) assigned to each of them. 

The ARCNET protocol employs an scheme known as token-passing, to manage the data 
stream among the active nodes in the network. When a node possesses the token, it can 
transmit data through the net or pass the token to the neighboring node, which, even though 
it can be located physically in any place, it has a consecutive node number. The header of 
each sent packet includes the address of the receiver node, in a way that all the nodes, with 
the exception of the pretended receiver, overlook those data. Once a message has been sent, 
the token passes to the next node, in a consecutive way, up to returning to the original node. 
If the message is very long (more than 507 bytes) it is split in several packets. The packets can 
be short or long depending of the selected mode. Short packets are from to 252 bytes, and 
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the long ones from 256 to 507 bytes. It is not possible to send packets of 253, 254 or 255 bytes; 
in those cases null data are added and they are sent either as two short packets or one long. 
As each node can only send data when it has the token, no collisions occur when using the 
ARCNET protocol. Moreover, thanks to the token-passing scheme, the time a node lasts in 
sending a message can be computed; this is an important feature in industrial networks, where 
it is required that the control events occur in a precise moment. 

5.1.1 Specifications for the PA1 system 

Communication between the motion control board and the PAlO's motor drives is carried out 
via an optical fiber, using the ARCNET protocol. In its original configuration a transmission 
rate of 10 Mbps is employed; the node number of the ARCNET controller IC in the driver 
cabinet is 254 (hexadecimal FE), while the one in the motion control board has number 255 
(hexadecimal FF). As explained in (Mitsubishi, 2002b), the data packets are short, and they 
follow the format which is shown in Figure 4. 



+0 +1 +2 +3 



+X 



+255 



Data type 
Sequence No. 

-Sequence No. address (=X) 

- Receiver node number 

- Transmitter node number 



Transmission data 
(even number) 

ARCNET 

Short-packet 
format 



Fig. 4. ARCNET data transmission format 



The first two bytes in each packet indicate the node numbers of the transmitter and receiver, 
respectively; next byte specifies the address (X) in which the data block start, in a way that the 
last byte always be the number 256. 

The first byte of data (identified as "Data type" in Figure 4) can be either 'S', 'E' or 'C. Com- 
mands 'S' and 'E' indicate, respectively, the start and end of a control sequence; when using 
these commands, no more data is expected. Command 'C is used to transmit control data, 
which must be done periodically. When executing the 'S' command, a timer starts its opera- 
tion, which checks that the time between each 'C command does not exceeds a preestablished 
time limit; in case of occurring this, an alarm signal is activated, and the operation of the robot 
is blocked. The sequence of commands for normal operation is 'S'—¥'C'—¥ ■ ■ ■ — >'C— >'E'. 
The control computer (either using the motion control board or not) must send periodically 
the necessary signals to the servo-drives; these must acknowledge, returning to the computer 
the same command that they receive. 

When executing the 'C command, the control computer sends 44 bytes of data to the motor 
drives, being two bytes with general information regarding the communication and six bytes 
with particular information for each of the seven servos; among the latter, two bytes contain 
flags for turning on/off the servo, activating the brake, selecting the torque /velocity mode, 
etc.; the other four bytes indicate the desired torque and velocity for the corresponding actu- 
ator. On the other hand, 72 bytes are transmitted from the drives to the computer, being two 
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with general information and 10 for each servomotor, among which we find the status data 
(on/off, brake, etc.), and the actual joint position, velocity and torque of each motor. More 
details on this can be found in (Mitsubishi, 2002b). 

It is important to highlight that, when using the motion control board and the PA library 
functions, the user has no access to all this information, and, in fact, he cannot configure the 
robot in torque mode. In the following subsections we describe the hardware and software 
interfaces we developed for overcoming such difficulties. 

5.2 Hardware interface 

In order to work with the PA10 arm in torque mode, it was necessary to have access to the sig- 
nals commanded to the motor drives. For doing so, we decided to use another PC, equipped 
with an ARCNET board. This has been done by other authors, for example (Jamisola et al., 
2004). 

In (Contemporary, 2005) it is explained how a PCI22-48X ARCNET board from Contempo- 
rary Controls can be modified to make it work with the PA10 robot. In the same document 
it is mentioned that even though the PCI22-485X boards are now discontinued, the replace- 
ment parts are the PCI20U-485X and the PCI20U-400, from the same brand; all of them have 
the same ARCNET controller (COM20022); the only difference is the circuit for transmis- 
sion/reception (known as transceiver), which is in the daughter board soldered to the base 
board. 

For the application presented in this work, we acquired a PCI20U-485X board, which was 
installed in the PCI bus of a PC with a double-core processor, running at 2.4 GHz. This board 
was configured with the same ARCNET setup of the motion control board. On the other 
hand, for the sake of compatibility, we made an optical interface board, similar to the one in 
the original PA10 system. 

Following the steps in (Contemporary, 2005), the daughter board with the transceiver was 
withdrawn from the PCI20U-485X and in its place we put a connector for the cable linking the 
signals from the main board with the optical interface; this latter plays the role of transceiver 
and converts the electric signals in optical signals to be transmitted by the optical fiber. 
Figure 5 shows the connections required to use the PCI20U-485X board. 



SI I 
I* 



Optical interface 
board made in ITL 



20 pin 
connector 



D 




\ 



PCI20U-485X 

Fig. 5. Connection between the ARCNET board and the optical board 
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5.3 Real-time software 

Real-time control of industrial processes has become an important topic in the recent years. 
Real-time systems are those in which it is imperative that their response occurs within a limit 
time. There are real-time operating systems that facilitate the development of this kind of 
applications. Microsoft Windows is not a real-time operating system, but it can behave as if 
one, by using an additional software, such as the RTX (real-time extension) from IntervalZero 
(formerly Ardence). 

RTX allows Windows to handle two type of executable programs at the same time: (a) those 
with extension .RTSS, which are handled completely by the RTX real-time subsystem, thus 
being more predictable; and (b) those with extension .EXE, handled by the basic Windows 
subsystem (Win32), which are not so severe with time constraints, but they can include RTX 
functions and also use graphic interfaces. Thanks to RTX it is possible to simultaneously 
execute several real-time programs, and share information among them, by means of a shared 
memory. 

To develop real-time applications with RTX, the Visual C++ IDE is required. It was with this 
platform that we designed the software interface to control the PA10 robot, without using the 
motion control board. Figure 6 shows a general diagram of the components of the developed 
software. 



Main window 
(ARCNET comunication) 




Simulators 







Torque 




position 
















Velocity 




position 




















position 








Virtual World 





Fig. 6. Components of the control software 

The central part of the system is the interchange of information among the different programs 
via shared memory. These programs are briefly described in the following subsections. 



5.3.1 Main window 

When starting the main program the user is being asked on what operation mode (velocity 
or torque) he wants to use for controlling the robot. After that, the main window of Figure 7 
shows up; by using this window the user can select the control algorithm to execute, as well 
as enabling the simulator and the virtual world; further, the window is useful for monitoring 
and, if necessary, modifying the torque or velocity signals. 
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Fig. 7. Main window of the control software 



It is important to mention that this program establishes the communication with the servo 
drives via ARCNET. This communication starts and stops by pressing the "Start Robot" and 
"Stop Robot" buttons in the window. 

5.3.2 Simulators 

Depending on the operation mode selected, a simulator of the robot can be executed, which 
resolves the corresponding model in order to obtain the joint coordinates of the robot, starting 
from the torque or velocity input. 

5.3.3 Control algorithms 

They are a series of Visual C++ programs which implement different control laws, which 
can be selected depending on the operation mode selected at the beginning. The number of 
programs increases when new controllers are added. 

5.3.4 Virtual world 

It shows, via a 3D animation, the behavior of the robot in real-time, either in simulation or in 
a real experiment (see Figure 8). 

6. Experimental Results 

In order to evaluate the performance of the developed real-time software for controlling the 
PA10-7CE robot arm, we carried out some experiments. The idea was to test the implementa- 
tion of the two-loop controller studied in Section 2.2 in three different operation modes. 
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Fig. 8. Virtual world of the PA10 robot 

Summing up, the following three cases were considered, being the difference how the in- 
ner and outer controllers were implemented (either in the control computer or in the servo- 
drives): 

A. Both RMRC and PI controllers in the real-time software; drives in torque mode. 

B. RMRC controller in the real-time software; PI in drives, configured in velocity mode. 

C. Both RMRC and PI controllers in the original system, with the motion control board. 
The control aim of the experiments was to track a desired time-varying trajectory. In order to 
show the benefits of redundancy in the PA10-7CE, a secondary task was designed, consisting 
in maximizing the distance of the joint positions to the actuator limits. 

6.1 Desired trajectory 

The primary task consisted in tracking a straight line in Cartesian coordinates, subject to the 
following constraints: 

• The robot's end-effector should move only along the Y axis, with respect to the base 
coordinate frame; hence, x and z coordinates should remain constant. 

• The end-effector should perform a turn around the Z axis of the base coordinate frame; 
hence, /3 and 7 should remain constant. 

• Variable operational coordinates (y and a) should perform a cycloidal-type motion, 
which is given by the following expression (Sciavicco & Siciliano, 2000): 



j(f) =S, + {Sf-Si) 



In 



2nt 



(54) 



where s(t) is the corresponding operational coordinate (as a function of time t), s, and 
Sf are, respectively, the initial and final values of such s(f), and T indicates the duration 
of the cycloidal motion. 



Motion Control of Industrial Robots in Operational 
Space: Analysis and Experiments with the PA10 Arm 



437 



As explained in (Sciavicco & Siciliano, 2000), the cycloidal motion given by (54) produces 
smooth velocity and acceleration profiles, which is a convenient feature in robotic tasks. 
For the experiments, we chose the parameters in Table 1. 



Parameter 


Value 


Unit 


X 


-0.339 


m 


Vi 


-0.339 


m 


Vf 


0.339 


m 


z 


0.837 


m 


«i 





rad 


CCf 


1.5708 


rad 


P 





rad 


? 





rad 


T 


20 


s 



Table 1. Parameters for the desired trajectory 



6.2 Secondary (redundant) task 

The PA10-7CE arm is a 7-dof redundant manipulator for pose control tasks (requiring only 6 
dof). Thus, we decided to take advantage of the redundant degree of freedom to perform a 
secondary task, which should not affect the primary task (tracking of the desired trajectory) 
but only the self-motion of the robot joints. 

According to the analysis in Section 2, the secondary task is added to the controller by means 
of vector q Q in (13). Among the different methods of choosing vector q , those considering it 
as the gradient of a suitable cost function, H(q), are the most common (Sciavicco & Siciliano, 
2000). For the purpose of the experiments in this chapter, we chose the following cost function: 



H(q) 



7 



(55) 



where qi — cji is the range of operation for the 2-th joint, and q\ is the central value of this 
range. Minimizing this cost function implies keeping each of the joints near to their corre- 
sponding central values, that is, far from their joint limits. 

In order to minimize (55) we should make q Q equal to its negative gradient; in other words, 
the 2-th element of q should be 



% 



-A 



(<7w - <?%«, ) 2 



where A is a weighting factor. 

For the experiments we used the actual joint limits of the robot, shown in Table 2, with q; = 

— qi. , and q\ = for each link i. 

6.3 Results 

We carried out three sets of experiments, each to test the performance during the execution 
of the two-loop controller shown in Figure 1, but using different operation modes. The same 
primary and secondary tasks (mentioned previously) were chosen for the three cases. The 
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Joint 


Limit 


Unit 


1 


180 


deg 


2 


97 


deg 


3 


180 


deg 


4 


143 


deg 


5 


270 


rad 


6 


180 


deg 


7 


270 


deg 



Table 2. Joint limits for the secondary task 



gains of each controller were tuned so that a good performance of the tracking task could be 

perceived. 

Prior to executing the desired task in operational space, a joint position controller was used to 

move the robot to the required initial pose. 

6.3.1 Case A 

In this case, the two controllers (outer RMRC and inner velocity PI) where implemented in the 
developed real-time application, using a sampling period of 10 ms. The control gains were 
chosen as follows, 



RMRC controller: 
PI controller: 



K = diag{40,50,40,12,12,12} 



K p = diag{80,120,40,48,5,8,15} 
K, = diag{36,50,24,20,10,10,10} 

The weighting factor for the secondary task was chosen to be A = 2.5. The servo-drives where 
configured in torque mode. 

6.3.2 Case B 

For this case, only the RMRC controller was implemented in the control computer, with the 
control gains: 

K = diag{24,24,24,8,8,8} 

The servo-drives were configured in velocity mode, so that the inner PI velocity controller was 
implemented in them. Factor A had to be reduced in order to obtain a better performance; its 
final value was A = 0.7. 



6.3.3 CaseC 

In this case we did not used the same software program in the control computer. Instead, we 
employed an application using a Windows multimedia timer and some API functions from 
the PA library, to send the desired operational coordinates to the motion control board. The 
control scheme was also chosen via API functions, in a way of implementing the RMRC in the 
control board and configuring the servo-drives in velocity mode so as to use their inner PI 
velocity loops. 
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6.3.4 Discussion 

In order to compare the performance of the three cases, we decided to compute the Euclidean 
norm of both the position error \\p\\ and the orientation error || ip || . Figures 9 and 10 show the 
evolution of such norms for the three cases. 
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Fig. 9. Time evolution of the norm of the position error. 
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Fig. 10. Time evolution of the norm of the orientation error. 
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Figures show that even though Case C corresponds to the original setup for the PA10, in the 
case of the position error, we obtain a better performance when using the drives configured 
in velocity mode (Case B). As shown in Figure 10, the orientation errors for cases B and C are 
quite similar. Regarding the results of Case A (drives configured in torque mode) we notice a 
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more irregular shape of the norms. Some error peaks appear in the last half of the trajectory 

(when the velocity is decreasing); this is probably due to static friction, which appears at low 

velocities. 

To have a better appreciation of the performance for the three cases, we decided to use a 

standard index: the root mean square (RMS) value of the norm of the corresponding error, 

computed in a trip of time T, that is 



RMS(w) 



i r T 

r/o 11*^1 



2 da 



where u can be either p or ip. Table 3 shows the performance indexes obtained during the 
execution of the trajectory (T = 20 s): 



Index 


Case A 


Case B 


Case C 


RMS(p) 


4.379 


1.816 


6.065 


RMS{$) 


0.021 


0.015 


0.014 



Table 3. Performance indexes 

It is worth noticing that the best performance is obtained with Case B (velocity mode). Further, 
even though Case A (torque mode) seems to produce more oscillations (see figures 9 and 10), 
the performance index for the position is lower for Case A than for Case C. 

7. Conclusions 

In this chapter, we have dealt with the motion control problem in operational space, using 
the resolved motion rate controller RMRC (kinematic control) plus the intrinsic joint velocity 
PI controller of the industrial robots, whose solutions of the overall closed-loop system have 
been proved to have uniformly ultimately boundedness. 

Due to its open architecture system, the PA10 has become a suitable robot for both research 
and industrial applications. This leads to the need of studying the operation of each of the 
sections that compose its hierarchical structure. 

Some experiments were carried out in a PA10-7CE arm, using a software program we have 
developed for control in real-time. The same hierarchical structure was tested in different 
operation modes. The results show good performance in all cases, even though the operation 
in torque mode is more affected by mechanical vibrations, perhaps due to friction and to the 
discretization of the controllers. 
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1. Introduction 

Magnetic resonance imaging (MRI) is a non-invasive medical imaging tool that helps 
physicians diagnose and treat medical conditions. It offers excellent visualization of soft- 
tissue in any imaging plane without using of iodinated contrast agent or ionizing radiation. 
In addition to anatomical morphology, MRI can also provide functional information. 
With novel fast imaging technologies, MRI became an imaging tool for guiding and 
monitoring various interventions and biopsy procedures on various organs including the 
brain, breast, prostate, liver and spine (Jolesz, 1998, Melzer & Seibel, 1999). High 
performance magnetic field gradients, multi-channel receivers, and advanced reconstruction 
systems improve the clinical applicability of real time MRI procedures (Nayak et al., 2004, 
Bock et al., 2004, Guttman et al., 2003, Lederman, 2005). Modern scanners designed for the 
interventional environment can provide real-time images of acceptable quality in excess of 
10 frames per second. As a result, real-time MRI (rtMRI) is becoming an attractive method 
for many minimally invasive cardiac interventions (Kuehne et al., 2003, Raval et al., 2006, 
Henk et al., 2005, McVeigh et al, 2006, Horvath et al, 2007). 

However the confined physical space of MRI scanner challenges medical intervention, even 
the magnets with open architecture provide only a limited working space, at the expense of 
image quality. The use of robots inside the MRI scanner is a very attractive solution: a robot 
manipulates the intervention instruments while MR images continuously give feedback of 
the position of the instruments which are controlled by the robot. MR compatible robotic 
systems have been researched and developed for prostate biopsy and brachytherapy 
(Chinzei et al., 2000, Krieger et al., 2005, Fischer et al., 2006, Stoianovici et al., 2007a), breast 
intervention (Kaiser et al., 2000, Larson et al., 2004), interventional spinal procedure 
(Hempel et al., 2003), neurosurgery (Masamune et al., 1995, Koseki et al., 2002), 
interventional liver therapy (Hata et al., 2005, Kim et al., 2002), and cardiac intervention (Li 
etal., 2008). 

Design of a system operating inside or close to the bore of a high field MRI scanner is of 
significant complexity. Due to the strong magnetic field of the MRI, standard materials, 
sensors and actuators cannot be employed. Due to the confined space of MRI bore, the 
mechanical design should be compact and simultaneously functional. In this chapter, we 
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discuss MRI robots for medical interventions, including the MR compatibility, mechanical 
design and control method. As an example, we present our work on the development of a 
robotic valve delivery module for transapical aortic valve replacement under real-time MRI 
guidance. Furthermore, we present our work on an MR compatible hands-on cooperative 
control of a pneumatically actuated robot. 

2. MR compatibility 

Three basic features for high quality MRI are high and homogenous magnetic fields, fast- 
switching magnetic field gradients and radiofrequency (RF) pulses. These extreme 
environmental conditions require adequate construction materials, actuation assemblies, 
sensors, and proper shielding of electronics. For example, in strong magnetic fields, 
ferromagnetic materials can be dangerous projectiles. Also, the homogeneity of the main 
magnetic field is strongly affected by ferromagnetic materials inside the MRI scanner, 
resulting in substantial distortion of images. Fast changing gradient magnetic fields can 
induce electrical fields and eddy-currents inside conductive materials. These eddy-currents 
can heat conductive materials and distort the homogeneity of the main magnetic field and 
thus severely affect the quality of MR images. RF pulses can heat conductive elements, such 
as wires or interventional devices. Devices containing closed metallic loops should be 
avoided in the MR environment. 

MRI is also very sensitive to electromagnetic noise. Electronic equipment and wires needed 
for the operation of mechatronic devices placed inside the MRI scanner room can generate 
electromagnetic noise if these are not properly shielded. 

MRI compatibility of robots has different levels: 1) MR safety, no risk to the patient or 
medical personnel; 2) image quality not affected by the introduction and operation of the 
robot; and 3) the operation of the robot not affected by the magnetic fields (Schenck, 1998, 
Schenck, 2000, Chinzei, 1999). We present MR compatibility of different materials, actuators 
and sensors and discuss the level of the compatibility of each of these in this section. 

2.1 Materials 

Based on their reaction in a magnetic field, the materials can be characterized in to three 
categories: ferromagnetic, paramagnetic and diamagnetic materials. Ferromagnetic 
materials, such as iron, magnetic stainless steel, cobalt and nickel, exhibit a strong attraction 
to a magnetic field. These materials are not MR safe. They could be a dangerous projectile to 
patients, physicians, and the MRI scanner when they are close to the scanner. Moreover, 
ferromagnetic materials create positive susceptibility to an external magnetic field. The 
homogeneity of an MR static field will be disturbed by the presence of ferromagnetic 
materials; therefore the image will be distorted. Yet, the ferromagnetic materials, such as 
magnetic stainless steel, have desirable mechanical properties. Paramagnetic materials, such 
as aluminium, platinum and austenitic stainless steels, become slightly magnetized when 
exposed to a magnetic field. Paramagnetic metals have a small and positive susceptibility to 
magnetic fields; hence they require some compromise when they are used inside an MRI 
scanner and close to imaging areas. Diamagnetic metals have a very weak and negative 
susceptibility to magnetic fields. Copper, silver, and gold are diamagnetic. They are MR 
safe, and will not affect the homogeneity of a static magnetic field when they are placed 
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close to an MRI scanner. They have very limited or negligible image artifacts even they are 
located close to imaging areas. 

Another effect of MR compatibility is the generation of the eddy-currents inside conductive 
materials due to fast-switching gradient magnetic fields. These eddy-currents may alter the 
local homogeneity of the main magnetic field and affect the quality and linearity of MR 
images, causing image artifacts. Moreover, eddy-currents may also cause unwanted heating 
of the materials. 

Materials suitable for MR-compatible devices in level 3 are nonmagnetic and 
nonconductive. Plastics, ceramics and composite material have been used in the 
development of MR-compatible systems. But plastics are often too soft and present limited 
structural stiffness, whereas, ceramics are too hard, brittle and could lead to manufacturing 
difficulties or part failures. Bearings, ball screws and gears made of non-ferromagnetic 
metals, such as aluminium, copper, brass, titanium, and tantalum are often used in MR 
compatible systems. These metal parts do not present substantial problems or image 
artifacts as long as they are of small size and appropriately positioned relative to the 
imaging areas. 

In conclusion, MR-compatible materials can be characterized into three categories 
depending on their magnetic compatibility when they are placed inside an MRI scanner: 

1) materials that produce no image distortion: nylon, vespel, PEEK (poly-ether-ether- 
ketone), polysulfone, zirconia, plexiglass. Polyimide (from class of vespel materials) for 
example, presents outstanding structural strength, good chemical resistance, very low 
electrical and thermal conductivity, low friction and have no detectable MR artifacts. 

2) materials that produce low level of image distortion, but for many applications are 
acceptable: brass, zinc, lead. 

3) materials that produce noticeable image distortion, but for some application are still 
acceptable: titanium, tantalum, tungsten, zirconium, molybdenum, aluminium. 

The MR compatibility of certain types of materials and devices have been reported in 
(Chinzei et al., 1999, Schenck, 2000). In order to be suitable for medical devices or medical 
robotics, these materials also should be biocompatible, sterilizable and economical. 

2.2 Actuators 

Electromagnetic motors are commonly used to actuate conventional robotic system, yet they 
are not safe and compatible in an MRI environment. The principle of electromagnetic motor 
operation is that a mechanical force is produced by the interaction of an electric current and 
a magnetic field. Electromagnetic motors usually are equipped with magnets to maintain a 
magnetic field. When such an electromagnetic motor is placed close to MRI bore, it will 
obviously disturb the homogeneity of the MR static field. On the other hand, the static 
magnetic field and the fast-switching gradient magnetic field will also affect the operation of 
the motor. 

Ultrasonic or piezoelectric motors are used in various MRI compatible robotic systems 
(Masamune et al., 1995, Chinzei et al., 2000, Kaiser et al., 2000, Koseki et al., 2002, Elhawary 
et al., 2007). These motors are driven by the ultrasonic vibration of a piezoelectric transducer 
when high-frequency voltage is applied; therefore theoretically they do not produce 
magnetic fields and are immune to the magnetic field. According to (Chinzei et al., 1999 and 
Fischer et al., 2008a), some commercially available ultrasonic motors are prone to cause 
image artifacts and significant signal-to-noise ratio (SNR) loss. The advantages of ultrasonic 
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motors include compact shape, small size, bidirectional and high torque output. The high 
breaking torque allows the robot to maintain its position even when it is not be powered. 
But these motors are not back-drivable, mechanical clutches need to be implemented to 
manually move the robot without detaching the mechanical part and the motor in the case 
of a medical emergency. For interventions which require simultaneous use of the imager 
and the robot, the ultrasonic motors should be placed outside of the MRI scanner. A motion 
transmission system, such as shafts, belts/chains, cables and linkages, is required to transfer 
the motion from the ultrasonic motor to the area of interest. Remote actuation can present 
additional limitations such as, backlash, friction and joint flexibility. 

Hydraulic actuators use pressurized hydraulic fluid, such as oil or saline in medical 
environments (Kim et al., 2002). A hydraulic actuator includes a hydraulic cylinder, a piston 
with sliding rings and seals, valves, and a supply pressure. Hydraulic actuators can transfer 
large forces, but the leakage of fluid is an issue in a medical environment. Air bubbles in the 
fluid could create control problems. Hydraulic actuators are recommended for applications 
that require high position accuracy, or slow and smooth movements. 

Pneumatic actuators have similar construction to hydraulic actuators. Howerver, they use 
compressed air instead of liquid. Without the threat of the liquid leakage, they are cleaner 
and are more desirable for a medical environment. Pneumatic actuators can be operated at 
higher speeds than hydraulic actuators, but due to compressibility of air, they usually 
provide limited stiffness. Pneumatic actuators are suitable for relatively low -force 
applications, such as actuating a robot in soft-tissue intervention. 

Both hydraulic and pneumatic cylinders made of non-magnetic material can be safely 
placed close to the imaging areas and will not cause image artifacts. The control valve and 
control unit should be placed outside of MRI room, or at least outside of the 5 Gauss line. 
Pneumatic actuators recently have been used in MRI compatible robotic systems for real- 
time imaging guided intervention (Hempel et al., 2003, Fischer et al., 2008b, Li et al., 2008). 
Because of the inter-relationship between temperature, pressure and volume of air, the 
dynamic control of the pneumatic actuator is challenging. A new concept of pneumatic 
actuator-PneuStep was presented by Stoianovici and colleagues (Stoianovici et al., 2007b). 
PneuStep uses a stepper motor principle to achieve precise motion on the order of 0.050mm. 

2.3 Sensors 

Position and/ or force sensors are used to provide feedback for close-loop control of medical 
robotic devices to provide a safe and accurate operation. Sometimes redundant sensors are 
used to guarantee the safety in medical intervention systems. 

Optical signals do not interfere with magnetic field. Optical sensors have been developed 
and used for sensing position (Fischer et al., 2008b, Hempel et al., 2003, Hata et al., 2008) and 
force (Harja et al., 2007, Takahashi et al., 2003) in an MRI environment. A commercially 
available optical encoder from US digital (Vancouver, Washington, USA) uses transmissive 
strips for positioning the motion. The reading heads have small electronics to convert the 
optical signal into an electronic signal. The optical encoder functions well and does not 
cause image artifacts even if it is placed 5cm from the imaging areas (Fischer et al., 2006). 
Innomedic (Herxheim, Germany) has developed both rotary and linear optical encoders. 
These magneto-translucent fiber-optical incremental encoders are made of glass. Optical 
fibers transfer the signals to optoelectronic conversion circuits remotely located. These 
encoders do not cause image artifacts even if they are placed at the isocenter of the magnet. 
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Fig. 1. Mean percentage difference between images under different conditions and baseline 
image (Phantom only), (a) with robot only (b) with robot inside bore and sensor at 1.0 m (c) 
sensor moved to 0.5 m and (d) sensor moved to 0.25 m 

Another type of important optical sensor is for measuring force in an MRI environment. 
These optical sensors (Harja et al., 2007, Takahashi et al., 2003, Tada & Kanade, 2005) are 
used in MRI-compatible robots developed as haptic devices for neuroscience and 
diagnostics. But they use some electronics which may not function during imaging. Our 
group modified a commercially available "3D SpaceNavigator" (3dconnexion, Fremont, CA) 
to create a 6 degrees-of-freedom (DoF) user input sensor which can be placed close to an 
MRI bore (Kapoor et al., 2009). A "3D SpaceNavigator" optical signal is converted to an 
electronic signal via a signal PC board, which connects to a USB interface board. After 
carefully rewiring and shielding, this ergonomic force sensor functions well when it is 
brought up to 50cm of the bore. Figure 1 shows the percentage difference between images 
under different conditions with the baseline image. 

Force sensing can also be achieved by using hydraulics/pneumatics (Liu et al., 2000, Yu et 
al., 2007). In their work, Gassert and colleagues (Gassert et al., 2006) presented an MR 
compatible 1-DoF force sensor based on transmission of hydrostatic force and motion which 
is located outside the MRI room, where traditional electronics can then be used. Due to 
friction losses and compressibility, there is a significant dead zone in the force sensed 
output. However, the advantage of using their technique is that it allows use of the device 
during imaging. 
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3. Mechanical Designs 

Scanners with open configuration present a major advantage regarding the accessibility to 
the target area. Double-donut MRI scanner offers a 50cm vertical gap between two facing 
superconductive magnets. This gap can accommodate a physician and interventional robots. 
Another type of open scanner with vertical arrangement of magnet poles at 40-45cm 
distance, also offers good accessibility to the patient. Open-configuration MRI systems with 
low magnetic fields, in the range of 0.02-0.7 Tesla, result in a lower available signal and 
slower speeds of image acquisition. The most popular MRI scanners at this time are based 
on a single superconductive magnet with a horizontal bore. This type of scanner offers 
better quality imaging with real-time acquisition speeds. The major drawback of these 
scanners for interventions is the limited space inside the bore, which typically is 60cm in 
diameter and 200cm in length. A new generation of scanners (Siemens Espree) with short 
bore (120cm) and increased diameter (70cm) make intervention access and interventional 
robot assistance within the magnet quite feasible. 

MRI scanner configuration, the target organ, and the intervention procedure greatly impact 
the mechanical designs of MRI robots. MRI compatible robots for percutaneous biopsy, drug 
injection or radiotherapy seed implantation are currently an active research area. We discuss 
the mechanism of some of these robots and focus on a robotic valve delivery module for 
beating heart transapical valve replacement in this section. 

Chinzei and colleagues presented a robotic system for assisting a physician who stands in 
the limited space between the two magnets of a double-donut MRI scanner (Chinzei et al., 
2000). The base of the robot is mounted above the physician's head in the open MRI magnet 
and two rigid arms reach down into the surgical field. The robotic device consists of five 
linear motion stages arranged to form a 2-DoF orienting mechanism, which is attached to a 
3-DoF Cartesian positioning mechanism. The ends of the arms are linked to form a tool 
holder. Recently, Hata and colleagues developed a semi-active needle-guiding manipulator 
for microwave thermotherapy of liver tumors in a double-donut open MRI (Hata et al., 
2008). The manipulator is installed on the side of the patient table at the gap between the 
magnets. It consists of a 3-DoF Cartesian stage and an encoded passive remote center of 
motion configuration. The synergistic control keeps the virtual remote center of motion at 
the pre-planned target using encoder outputs from the needle holder as input to the 
motorized base stage. 

Koseki and colleagues developed a robotic system for endoscope manipulation working 
inside a vertical-magnetic-field open configuration MRI scanner (Koseki et al., 2002). This 4- 
DoF robot takes advantage of the available horizontal space between the vertical poles of 
such scanners. They adopted a 5-bar linkage mechanism for 2-DoF translational motions. 
The planar parallel mechanism reduces the size and enables the actuators to be distant from 
imaging areas. Tajima and colleagues (Tajima et al., 2004) introduced a master-slave MR 
robotic system for intraoperative diagnosis and minimally invasive surgery inside an open 
vertical MRI scanner. Each of the slave manipulators has three translational DoF that are 
located on its base and three rotational DoF on its arm component. The ultrasonic actuators 
and sensors were located a meter away from the imaging areas. A mechanism consisting of 
a pair of differential gears and wires was implemented to transmit movement to the tool tip. 
Masamune et al. first designed a MRI-compatible manipulator for stereotactic needle 
therapy (Masamenu et al., 1995) in a conventional closed MRI. The device was designed to 



MRI Compatible Robot Systems for Medical Intervention 449 

be mounted on the MRI table and above the head of the subject. It has a stereotactic frame 
with 6-DoF. The isocentric structure avoids collision with the patient. 

There are several reports on MRI compatible manipulators and/or robots for prostate 
biopsy and brachytherapy. These compact devices are mounted on the MRI table and below 
the torso. Krieger and colleagues developed a 3-DoF manipulator for MRI guided transrectal 
prostate (Krieger et al., 2005). The manipulator is equipped with active fiducial tracking to 
encode the position of the needle path and is remotely manually actuated by the physician 
from outside of the MRI scanner. The device is comprised of a rectal sheath, which is placed 
adjacent to the prostate in the rectum of the patient and a needle guide, containing a curved 
needle channel. Stoianovici and colleagues developed a 5-DoF MrBot actuated by a new 
type of pneumatic stepper motor for brachytherapy seed placement (Stoianovici et al., 
2007a). The MrBot robot is constructed in the form of a platform supported by articulated 
linear actuators in a 5-DoF parallel link structure. The patient is in the decubitus position 
and seeds are placed in the prostate transperineally. Fischer and colleagues described a 6- 
DoF robot assistant system for transperineal prostate needle placement (Fischer et al., 
2008b). The mechanism consists of two decoupled planar motions. Motion in the vertical 
plane is achieved using a modified version of a scissor lift mechanism that is traditionally 
used for parallel plane motion, whereas motion in the horizontal plane is accomplished by 
coupling two straight line motion mechanisms generally referred to as Scott-Russell 
mechanisms. 

For accessing the organs located in the middle of the torso inside a closed MRI scanner, long 
flat arm like structure are usually used. Kaiser and colleagues presented ROBITOM, the first 
MR-compatible robotic system dedicated to MR-guided breast biopsies (Kaiser et al., 2000). 
The entire assembly is a box-like structure that is anchored on top of the standard MRI 
scanner table. The patient lies prone on the elevated table with the breasts placed through 
two openings. Larson and colleagues developed a similar breast-dedicated MR-compatible 
robotic system (Larson et al., 2004) with multiple degrees of freedom and remote control. 
Their device is remotely actuated with ultrasonic motors and a motion transmission system 
consisting of shafts and universal joints. Innomotion (Innomedic GmbH, Herxheim, 
Germany) is a commercially available MR-compatible interventional robotic system, original 
developed for precise needle insertion for MR-guided therapy of spinal diseases. An arch is 
mounted to the MRI patient bed with specially made rails. An arm sitting on the arch takes 
advantage of the clearance between the patient and the bore. The robot has 5-DoF with a 
remote center of motion structure to place and orient an interventional tool. 

3.1 Robotic module for transapical aortic valve replacement 

Our group has developed a robotic system for transapical aortic valve replacement under 
real-time MRI guidance (Li et al., 2008). The robotic component consists of a 5-DoF robotic 
arm and a 3-DoF valve delivery module. The 5-DoF Innomotion robotic arm is employed as 
the 3-DoF valve delivery module holder. The compact valve delivery module is designed for 
manipulating and placing the prosthesis into a beating heart, while in the MRI scanner. The 
valve delivery module consists of a sterilizable valve delivery unit and an active mechanism 
that provides the essential manipulation of the delivery device for valve placement. 
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Fig. 2. Picture of the robotic system for MRI guided transapical aortic valve replacement. It 
shows an Innomotion arm for the positioning module and the prototype of the new 
developed 3-DoF valve delivery module affixed on the robotic arm. 



The valve delivery unit includes a delivery device, a trocar, and a trocar adaptor. The 
delivery device consists of a straight plastic rod, outside of which is a sheath protecting the 
prosthetic valve before it is deployed. The robotic active mechanism comprises one 
rotational joint (B) and two linear joints: the translation joint (A) and the insertion joint (C) 
(fig 2). The operations of linear and rotational joints are independent. 

The translation joint provides linear displacement of the delivery device along its axis. This 
joint is directly actuated by two parallel linear pneumatic actuators. The two-actuator 
structure guarantees balanced motion of a delivery device. The bodies of two linear 
actuators, together with a base plate, a top plate and two inner rails, form a rigid sliding 
frame. The sliding frame can slide on two frame rails. These two frame rails, along with a 
connecting plate (for mounting on a positioning arm) and a top plate form a rigid base 
frame. The position of the motion is sensed by a linear optical encoder that is attached to a 
frame rail of the rigid frame. This travel range of this joint is 90mm. 

The rotational joint rotates the delivery device around its axis. This changes the orientation 
of the prosthesis relative to coronary ostia before it is deployed. It is actuated by a linear 
pneumatic actuator attached on the base plate of the sliding frame. The linear movement is 
transmitted to the rotational movement by a rack-gear transmission. The rotation angle is 
sensed by a linear optical encoder. A sliding base rack actuated by the pneumatic actuator 
transmits linear movement to the gears of the rotation axis. The travel range of this joint is 
±60 degree. 
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The other linear joint is an insertion joint. This linear movement advances the prosthetic 
valve out of the delivery device. One central sliding stage with a semicircular groove 
actuated by a linear actuator can slide on the inner rails of the rigid sliding frame. The whole 
travel length of the insertion is 45mm, which guarantees the prosthesis be driven out from 
the sheath. This linear movement is also encoded with an optical linear encoder. 
The prototype of the valve delivery module is shown in Figure 2. All parts of the prototype 
delivery module are made from non-conductive plastic materials. Pneumatic actuators and 
optical sensors are used for operating and positioning each joint. The robotic module allows 
precise and repeatable positioning of a bioprosthetic valve in the correct location. The 
maximum measured force load of the translation stage, under 75 psi operation air pressure, 
was 34N; and the maximum torque of the rotation stage was 0.4Nm. With operational 
velocity of 10mm/ sec, the accuracy of translation stage was found to be 0.19±0.14mm. With 
operational velocity of 5deg/sec, the accuracy of the rotation stage was found to be 
0.46±0.27deg. With a load of ION (a reasonable force if we consider the friction and reaction 
force from a real case), the module moved smoothly and stably and the accuracy was in the 
same range. The presence and motion of the robotic module inside the scanner was found to 
have no noticeable disturbance to the image. The observed signal-noise-ratio (SNR) loss was 
6.1% to 6.5% for the valve delivery module placed in the bore and in motion respectively. 

4. Control Strategies 

Improving precision and accuracy, while maintaining compatibility and safety with MRI are 
the prime concerns for most MRI compatible robotic systems. Point-to-point position motion 
is the basic control mode for these robots. However, a shared control between a user and a 
MRI compatible robot makes it a more intuitive instrument especially during the setup 
phases of interventions. We describe the needs and method to implement the hands-on 
coorperative control mode on a MR compatible robot in this section. 

Planning a minimally invasive intervention on living tissue is not a trivial task. The organs 
move around and thus the pre-planned motion based on pre-operative images alone is not 
sufficient. 




Fig. 3. Hands-on cooperative control 



Most of the contemporary systems make use of intra-operative images and a graphical user 
interface to update the planned motions. It may be not necessary and sometimes impractical 
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to have an image guided interface for the robot during procedure setup. In the robotic 
assisted intervention of beating heart transapical valve replacement (Li et al., 2008), before 
the prosthetic valve delivery process can be started, the surgeon must perform preparatory 
procedures of placing the trocar into the apex of the heart. Thereafter, the surgeon loads the 
delivery device with the prosthetic valve and inserts the delivery device into the trocar. 
Using imaging to guide the placement at this stage was unnecessary and impractical 
because of the large motion and variability in the localization of the trocar. Typically, the 
trocar port is about 10-15cm from imaging center, thus requiring a very large image 
acquisition volume. Breathing motion of up to 20mm also causes localization and 
registration errors. Moreover, adjustments may also be required to the entry point after a 
preliminary scan of the delivery device is acquired. 




100 150 200 

Time (ms) 
Fig. 4. Position error in the PIV controller for one of the robot axis under two different 
modes, (a) Gains optimized for point-to-point mode (b) Gains for hands-on mode. 
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Fig. 5. (a) User input with amplitude of 3.5mm/s and corresponding actual Cartesian 
velocity (b) Average Cartesian velocity error over time (2 cycles) with respect to amplitude 
of user input. 



Figure 3 shows the setup of the Innomotion robotic arm and user input sensor in the MRI 
room. We describe the method to implement the hands-on coorperative control mode on a 
modified pneumatically actuated Innomotion robotic arm (Innomedic, Herxheim, 
Germany). The actuators of the Innomotion robot are controlled by an off-the-shelf 
controller (Motion Engineering Inc/Danaher Motion, Santa Barbara, CA). A PIV 
(proportional position loop integral and proportional velocity) controller runs on the 
proprietary DSP based hardware. 
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4.1 Low level control 

One important difference between hands-on control mode and other modes is that the later 
prefers zero or low steady state errors. Hands-on mode has a user in the control loop who 
can tolerate a fair amount of steady-state error by adjusting his/her input to achieve the 
desired result. This mode demands a fast and smooth response without oscillations during 
the transient as well as steady state. Low level PIV gains should satisfy this requirement. It 
is straight forward to obtain such a response on traditional motor driven robots. However, 
for a pneumatic robot a compromise between smoothness, speed and error has to be made. 
To achieve this behavior, we applied a sine wave as user input (velocity) with varying 
amplitudes. Iteratively, we adjusted the PIV gains and the maximum amplitude of user 
input that would produce reasonable error, till we saw no further improvement. In figure 4, 
the fast oscillatory response for point-to-point gains is shown along with a smoother, 
reasonably fast response for hands-on mode. Figure 5(a) shows a typical profile for a user 
input with amplitude of 3.5mm/ sec and the actual Cartesian velocity measured using robot 
encoders. The error is defined as the difference between the user input and the actual 
Cartesian velocity at that instant. The average error is computed by averaging the error over 
the number of samples collected during two cycles of user input. Figure 5(b) shows this 
value for different amplitudes of user input. Our current system can tolerate a maximum 
user input speed of 3.6mm/ sec and 3.6deg/sec, if the required error is to be kept below 
0.5mm/sec and 0.5deg/sec. 

4.2 High level admittance control 

Depending on how the inertias and friction forces compare with the forces applied by the 
environment to the robot, it is possible to classify robotic devices into two categories: 
impedance-type and admittance-type. More importantly, it is the control scheme that 
distinguishes the two types. The impedance- type robots act as a force/ torque source, where 
the controller outputs a force based on desired input positions (and their derivatives). In 
admittance-type robots, the controller outputs a desired position (and its derivatives) based 
on user inputs such as force measurement or joystick motion. Innomotion robot is 
practically "non-back-drivable", i.e., it requires significant effort to overcome internal 
friction to maneuver it. Thus, it is more suited for an admittance control scheme such as 
those explored with the Johns Hopkins University Steady-Hand Robot (Taylor et al., 1999). 
In their work, force sensors could be used to obtain user input. However, MRI compatibility 
poses certain challenges in obtaining a 6-DoF user input. The outline of our hands-on 
controller is as follows: 

1) Switch the low-level PIV gains to ones optimized for hands-on mode. 

2) Obtain incremental motion desired by the user, that is x = K c ■ f , where x is desired 

incremental motion, f e R is user input and K c is a scaling matrix. 

3) Compute current joint state, q , from current actuator values, a , and incremental 

motion in the actuators, Aa . 

4) A constrained least squares problem is solved for the optimal incremental joint 
motion, Aq'by the high-level controller. The least-squares (LS) problem has an 
objective function describing the desired outcome. It includes constraints that consider 
joint limits, and importantly velocity limits. 
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Aq* =argmin||^ -(Ax-K f)\\ + \\w ■ Aq\\ 
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s.t. \q\ < q v ■ At 

q L <q + Aq< qu 

Ax = J ■ Aq 

The incremental joint motion, Aq , is a variable for LS problem. Matrix J is the 
Jacobian of the robot. q L and q u are the lower and upper bounds of the joint variables. 
q u is the upper bound of the joint velocities that are obtained as described in earlier 
section. At is the small time interval of the high-level control loop. Without any 
constraints, the above constrained LS problem, which is implemented in the high-level 
block, is equivalent to resolved rate control. W c and W are diagonal weight matrices. 

5) Numerically integrate the incremental joint motion to arrive at a new set of joint 
positions. We assume that for each iteration loop, the incremental motions are 
sufficiently small and Ax I At = J ■ Aq I At represents a good approximation to the 
instantaneous kinematics. 

6) Compute desired actuator values, a and desired actuator velocities, 
a' = Aa' I At from q and Aq* . These are new set points for the low-level controller. 

7) Repeat steps 2-6 while in hands-on mode. On exit, switch PIV gains to point-to-point 
mode values. 

Numerical integration and rate control laws such as these are known to be "non- 
conservative" and may result in positional errors. However, this is not a concern here, since 
a human-in-the-loop can easily account for any positional error by applying required 
additional input to reach the desired goal. The desirable behavior here is that the robot 
continues to follow the user input as best as it can, even in the advent of certain limits, such 
as joint or velocity extremity. 

4.3 Experiments and Results 

We tested the system functionality, as well as, the qualitative ease of use of the sensor. We 
also evaluated the hands-on sensor interface with respect to current and alternate interfaces. 
We used a quintessential peg-in-the-hole task to emulate the clinical scenario that inserts a 
dexterous tool into a trocar inside a thoracic cavity. Our peg was a cylinder 12.5mm in 
diameter and 140mm long, which mimicked the dimension of the delivery device. To design 
our experiments to be in line with Fitt's law (Zhai, 1995), we used two different hole 
dimensions (13.5mm and 16.5mm) for fine and coarse positioning accuracy, respectively. 
Further, the larger hole had a larger bevel at the entry point. The hole was placed at a 
known position with respect to the robot and the starting configuration of the robot was 
chosen at random for each trial. The starting position was chosen using an uniform random 

distribution from a spherical annular region of radii 15V3 to 20v3 mm, and the 
orientations were picked in the interval of ±15 - ±20deg. These are clinically relevant 
distances, because the robot can be easily and quickly positioned to within these regions 
using available passive adjustments. 
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For comparision, we developed a simplified GUI that resembles the commands acceptable 
to the AESOP, that is, the surgeon could annunciate one of "move"-"left", "right", "up", 
"down","front","back", or "rotate" -"left", "right", "front" and "back" to move the 5-DoF 
robot in the appropiate direction. The person at console would have to press the 
corresponding button. The buttons to increase/ decrease speed would change the current set 
speed by 0.5mm/ sec. In our experimental setup, the person at console could not directly 
visualize the robot to replicate the clinical setting. The person at console was picked at 
random from the pool of users to avoid bias. 

Each user was asked to perform the task as quickly as possible, on the "start" cue of the 
person on console and indicate verbally when they completed the task by announcing 
"done". Comparisons of task times recorded using a stop watch have been shown in figure 
6. As seen, hands-on approach is efficient for both the levels of accuracy. 
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Fig. 6. Histogram of time required to complete the peg-in-hole task (a,b) hands-on and hole 
size of 16.5mm and 13.5mm, respectively (c,d) AESOP like interface and hole size of 16.5mm 
and 13.5mm, respectively. The solid curves are the fitted Gaussian distributions with means 
and deviations as indicated. 



This can be attributed to the ability to perform complex continuous motions involving 
multiple translational and rotational degrees of freedom at one time. Further, using the GUI 
interface, the user announcing the commands has to contemplate his/her next moves. As 
expected, there is an increase in time required with an increase in the difficulty of the task. 
Though, this difference is not significant when compared with the difference between the 
two approaches. 



5. Conclusion 

MRI provides excellent visualization of soft tissue, its sub-structure and surrounding 
tissues. The unique features of MRI, such as oblique image planes, multi-slice imaging, real- 
time visualization, and freedom from radiation exposure risk, enable MRI to be a critical tool 
in the guidance of many interventional procedures. Additionally, a mechatronic system can 
provide more accurate and smooth access to targeting organs in a confined space. The 
marriage of a MRI and a robot makes the benefit of minimally invasive interventions 
substantial. 
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The high magnetic field and the confined space of MR bore presented many technical 
challenges. The design of robotic system is dependent of MRI scanner configuration type, 
medical intervention and target areas. The mechatronic components including actuators, 
sensors and controllers must be able to work in accurate, stable and robust way in MR 
environment. Materials used for robotic system should have low magnetic susceptibilities 
(comparable with air, water or human tissue), low electrical conductibility, adequate 
mechanical strength and good manufacturing properties. Certain conductive materials can 
be used, but only for small parts and without loops. If it is necessary, electrical components 
may be brought into MR environment if their electrical signals are of low frequency and are 
shielded. 

Control strategy and human machine interface for MRI compatible robotic systems for 
medical interventions need to be studied. A shared control between a user and an MRI 
compatible robot makes it a more intuitive instrument especially during setup phases of 
interventions. An image-guided interface would be ideal to plan and manipulate the robotic 
holder or adjust the interventional tool when the robot is inside the magnet bore. In the 
engineering of robots for medical applications, detailed analyses of the functions of the 
entire system, that is, robot, interfaces and application, taken as single entity, are arguably 
more important than the individual performance of the subsystems (robot, surgeon, 
interfaces and application, separately). Thus, having a combination of more than one 
interface such as an image-guided, console guided or hands-on based on application might 
yield a higher performance from the entire system. 
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1. Introduction 

Parallel robot manipulators comprise a mobile platform connected to a fixed base through 
three or more articulated links and are used extensively throughout industry for such 
diverse applications as high-precision positioning systems, fiber alignment, welding, robotic 
manipulators, automatic inspection systems, and so forth. Therefore, planning a trajectory 
to perform a specific task is one of the most important class of problems in the applications 
of the parallel robot manipulators. 

However, while moving along a specified trajectory, due to the limits on the workspace and 
existence of the force singularities, the parallel robot manipulators may not oppose forces or 
moments at some configurations. As a consequence, the manipulator gains some degrees of 
reedom, and becomes uncontrollable. Even the manipulator is very near to a singular 
manifold, the leg forces will increase violently to reach their allowable limits. Therefore, it is 
meaningful to plan a path without crossing a singular manifold on any operation for the 
parallel robot manipulators. 

Compared to the vast researches on the path-programming of serial manipulators, studies 
on the singularity-free path programming of the parallel robot manipulators are relatively 
few. (Bhattacharya et al., 1998) developed an exact and an approximate on-line singularity 
avoidance method to restructure a path in the vicinity of a singular manifold for platform 
type parallel manipulators. (Dasgupta & Mruthyunjaya, 1998) proposed an algorithm to 
obtain a singularity-free trajectory for given two end-poses. The continuous paths are 
constructed through well-conditioned via points by examining the condition number at 
discrete steps on the corresponding straight line segment. (Sen et al., 2003) used a variational 
approach based on a Lagrangian to plan singularity-free paths with the actuators lengths 
remaining within their allowable limits. (Dash et al., 2005) used local routing method based 
on Grassmann's line geometry to avoid isolated singularities inside the reachable workspace 
of parallel manipulators. 

However, at present, most researches on the singularity-free path programming only deal 
with the kinemetics of the parallel robot manipulators. When repetitive tasks in industrial 
applications are considered, some of physical operating costs, such as actuating forces or 
energy consumption, will become significantly important. Consequently, these effects 
should be further taken into account for a singularity -free path programming. 
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In this chapter, the dynamics formulation of a general parallel robot manipulator with an 
arbitrary geometry and inertia distribution according to the Boltzmann-Hamel-d'Alembert 
formulation will be first described. Then, the developed dynamics model in terms of the 
task-space coordinates are used to implement the singularity-free path programming for 
given two end poses of the parallel robot manipulator by minimizing some cost functions 
such as actuating forces, travel time and energy expenditure. 

2. The Boltzmann-Hamel-d'Alembert formulation 

2.1 Dynamic equations in terms of hybrid space coordinates 

When planning a singularity avoidance path with the minimization of a cost function, e.g. 
actuating forces, travel time and energy consumption, etc., the dynamic equations of the 
parallel robot manipulators should be developed for this purpose. Several methods such as 
the Newton-Euler formulation (Do & Yang, 1998), (Dasgupta & Mruthyunjaya, 1998), 
(Dasgupta & Mruthyunjaya, 1998), (Nakamura & Ghodoussi, 1989), (Nakamura & Yamane, 
2000), virtual work principle (Zhang & Song, 1993), (Wang & Gosselin, 1998), (Tsai, 2000), 
Kane's method (Liu et al., 2000), kinematic influence coefficient theory (Wang et al., 2003) 
and the traditional Lagrangian formulation in generalized coordinates (Lebret et al., 1993), 
(Pang & Shahinpoor, 1994) are proposed for modeling and simulation of the dynamics of 
parallel manipulators. Comparing these approaches each other, it is obvious that the 
Lagrangian formulation has more physical insight because it utilizes the kinetic and 
potential energies to generate a well-structured form of equations of motion. However, if 
generalized coordinates are selected to express rotations of a structure in space, the partial 
derivatives of the Lagrangian by Lagrangian formulation in terms of generalized 
coordinates are quite tedious due to more intensive symbolic computations on time-varying 
inertia matrix. Therefore, such a direct formulation in all generalized coordinates is so 
arduous and cumbersome that the dynamic equations were developed most only under 
some simplifying assumptions regarding the geometry, configuration, structure and inertia 
distribution of manipulators. Rather than using all of generalized coordinates for the 
dynamics formulation, here the angular velocities as the time derivatives of a set of quasi- 
coordinates instead of generalized coordinates are utilized. Such angular velocities 
expressions are so-called quasi- velocities. The formulated kinetic and potential energies are 
expressed in matrix forms as function of quasi- velocities and rotation matrices, and then the 
Boltzmann-Hamel-d'Alembert formulation is employed to derive closed-form dynamic 
equations of the parallel robot manipulators with a completely general architecture and 
inertia distribution. 

Fig. 1 illustrates the geometric structure of the general parallel robot manipulators 
considered in the present analysis. As shown, the mechanism has a fixed base and a moving 
platform connected to the base through six extensible legs with a spherical joint B ; at the 
top of the leg i (i= 1,...,6) and a universal joint A { at the base end. Each leg is composed of 

two segments, the lower segment 1 is a fixed part and the upper segment 2 is a moving part, 
both are joined together by a prismatic joint, and each prismatic joint is driven by a linear 
actuator. 
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Fig. 1. Schematic diagram of a general 6-UPS parallel robot manipulator 




ij -frame 
Fig. 2. Coordinate systems for one leg and moving platform 

Consider one of the six legs and the moving platform for a dynamic analysis as shown in 
Fig. 2, an inertial coordinate system N-frame, X N Y N Z N , is fixed at the base and another 
coordinate system B-frame, X b y b z b , is attached to the moving platform with its origin at the 
gravity center B of the moving platform. Two parallel local coordinate systems, i t -frame 
and i 2 -frame with their corresponding origins A n and A i2 , are attached to the respective 
lower segment 1 and upper segment 2 of the leg i. With the definitions of coordinate 
systems, the position vector of the gravity center of the moving platform with respect to the 
N-frame is denoted as p, the position vector of A n with respect to the N-frame is 
represented as a t , the position vector of the ball joint B ; with respect to the B-frame is 
defined as b f , and the length vector of leg i from the universal joint A n to the ball joint B; 
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with respect to the i t -frame is expressed as Z ; . Because each leg's length vector is equal to a 
total of the length vector, l n , of the lower segment 1 from A a to A i2 and the constant 
length vector, l l2 , of the upper segment 2 from A i2 to Ev , the length vector of leg i can be 
written as 



where s ( is a unit vector along the zth leg axis and expressed in the i a -frame; (V is the 
length magnitude of the leg i; l n and l i2 are the respective length magnitudes of the lower 
segment 1 and the upper segment 2 of the leg i. 

With multiple closed-loop chain mechanisms in a parallel robot manipulator, the geometric 
constraint equation on the basis of a vector loop relation can be written as 



\Rb : =a, + "Rl, 



(2) 



In Eq. (2), the rotation matrix N L R with the subscript L s {B,i(i = 1,.. .,6)} represents a 
transformation from a local L-frame to the inertial N-frame. The time derivative of a rotation 
matrix can be obtained by 

"R = "*A (3) 

in which a> L denotes a skew symmetric matrix formed from the angular velocity 
co L = o v CO a_ with respect to a local body-fixed L-frame such as 







-to, ®,, 
-a x 

co co 



CO 



(4) 



Moreover, the angular velocity expressed in a body-fixed frame can be defined in a set of 
quasi-coordinates /? t that are only defined meaningfully in angular quasi-velocities such as 



Ah 

dt 



(5) 



Differentiating the constraint equation, Eq. (2), yields the constraint equation in terms of 
velocities, the results is 



p + %Ra B b i = N i Ri i + N i Ra l l l 



(6) 



where I. = l n implies that each leg's sliding velocity is equivalent to the relative sliding 
velocity between these two segments of the leg. 
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Since each leg is connected to the base with an universal joint; therefore, no spin is allowed 
about its longitudinal axis, it gives 



mU< = 



(7) 



By pre-multiplying Eq. (6) respectively by ("Rs ; ) and by s { , the magnitude of the sliding 
velocity as well as the angular velocity of leg i with respect to the i t -frame can be calculated 
as (Chen, 2003) 

Ul^^Rs,) 1 p+^Rs,) 7 N B R3 B b { (8) 

^-y-Wfa+.RSA) (9) 



Because Eq. (8) relates the velocities of the ith active joint to the linear velocities and angular 
velocities of the moving platform, the Jacobian matrix can be formulated by writing six 
times for each i=\ to 6 and assembling in a matrix form as 



(10) 



p'l 






= Jacob 


P 


.4. 




_ W bJ 



where 



Jacob ■■ 



(~R S ,) T (~K S ,) T W 



C 6 Rs 6 f CRsJ TN B Rbl 



(11) 



Using the Boltzmann-Hamel-d'Alembert formulism (Chen & Chi, 2008), (Chen & Liao, 
2008), the dynamic equation of the parallel robot manipulator can be formulated in terms of 

the task space coordinates (i.e. x = \p J y T B , where y B is defined by a set of Euler angles 
specifying the orientations of the moving platform) as follows: 



M x + D x + G = f , 

p p p J p ' 



(12) 



where f is a function of the output forces of the linear actuators, / = [/, ■ ■ ■ f 6 1 ■ The 

matrices and vectors in Eq. (12) are fully derived in the Appendix. 

For the forward dynamics analysis, the trajectories of the manipulator can be solved 
numerically as a function of the actuating forces and initial states. For the inverse dynamics, 
the actuating forces are given as a function of the positions, velocities and accelerations of 
the moving platform. I.e., 



f = B~(M p x + D p x + G p ) 



(13) 
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where B = C[ (Jacob) 7 

It can be seen from Eq. (13) that a very large actuating force is required if the Jacobian matrix 
is rank-deficient or the value of the determinant of the Jacobian matrix approaches a 
singular manifold. 

2.2 Fundamental structure properties 

Although some fundamental structure properties of dynamic equations have been derived 

for open-chain manipulators in robotics, these derivations are of general nature based on the 

direct Lagrangian generalized coordinates formulation in an index form. In the sequel, these 

fundamental structural properties for the dynamics of the general parallel manipulators will 

be validated in a straight proof. 

Lemma 

For the task space dynamic equations, Eq. (12), 

1. The inertia matrix M is symmetric and positive definite. 

2. M -2C is a skew-symmetric matrix. 

Proof: 

Since the sub-matrices M, , M 2 and M are symmetric, (see Appendix), the symmetry 

property of M is easily seen by taking transpose on M such that M T =M . The kinetic 

energy of the parallel robot manipulator is equal to a summation of the kinetic energy of the 
moving platform and the six legs. Thus, 

K.E. = |* r M,i = hlM^+hlM^ (14) 

Therefore, the positive definiteness of M is guaranteed by the definition that the kinetic 
energy of the system is zero only if it has a zero velocity. 

-l. ro o _ 

To prove that M - 2C is a skew-symmetric matrix, we first show that D t = _ and 



M, - 2D, = 



D 2 

are all skew-symmetric. It implies that D 22 and M 44 -2D 4 



-2D 34 

-2D 43 M 44 -2D 4 

are required to be skew-symmetric matrices as well as D 34 = — D 43 . The properties that 
D T 2 - -D 22 and D T M - -D 43 are quite clear from the notation of these sub-matrices, (see 
Appendix). We then calculate 

M 44 -2D 44 =dx^[E 4 ,..., E 6 ] (15) 



where E. =m i2 / il (s;d 2 -d; 2 s.)-2(I n +I j2 )co. -2m i2 / n (« d. 2 )s. , i = l,...,6 

After taking transpose it is shown that(M 44 -2D 44 ) T = -(M 44 —2D U ) . So it is concluded that 

both D 4 and M 2 -2D 2 are symmetric matrices. 
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Next, it will be shown that M -2D is skew-symmetric. We obtain 

M p - 2D t , = -2D 1+ JU; T (M 2 - D,)/- 1 /, + ^(JUfW, (j, 1 /,) ~ JlJfM i J^h) (16) 

Now that the first two terms of the right hand side are the sum of skew-symmetric matrices, 
by taking transpose and using symmetry of M 2 , (M - 2D ) T = - (M - 2D ) is proved. 

Finally, M , — 2C is calculated as 

M p -2D p =C T t (M p -2D p )C 1 +C 1 M r Cl -ClM p C t (17) 

Because the first term of the right hand side has been proven as a skew-symmetric matrix, 
after taking transpose and inverting the sign, the skew-symmetric property of M — 2C for 
the task space dynamic equations is verified. 

3. Singularity-Free Paths Programming 

3.1 Cost function and constraints 

The objective here is to determine a path completely within the workspace with the 
following cost functions to be minimized while moving from the configuration x to the 
final configuration x. during the time interval from t to t f : 
(1) Minimum actuating force 

G=ti(i\f,\)dt (18) 



(2) Time optimum 



(3) Energy efficiency 



G=At = t f - t 
(19) 



G = l'Afl\dt (20) 



(4) Mixed cost function 

A cost function can be formulated by mixing Eqs.(18) & (20), or Eqs. (19) & (20) using a 

weighting coefficient whose value is in the range of zero and one. The weighting coefficient 

weights these two functions according to the relative importance. 

In addition, the considered trajectories in the task space must satisfy the constraints and 

conditions as follows: 

(l)Initial and final conditions: 

x(t ) = x , x(t.) = x f with the associated boundary conditions 
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x(t„) = x(t f ) = x(t a ) = x(t f ) = (21) 

(2)Leg length constraints 

/„„„ < / ,. (x) < /„„„ for i'=l„ . .,6 and t, < t < t, (22) 

(3)Leg's linear velocity constraints 

|i(*,*)| < /„,„,. for i=l„ . .,6 and t < t < t f (23) 

(4)Leg's linear acceleration constraints 

|'(.(x,x,x)| < /„„ ?v for i=l,.. .,6 and t o <t<t f (24) 

(5)Actuating force constraints 

|/;.(x,x,x)|</,„„ (25) 

Note that Eq. (25) restricts the output forces so that the planned path is implicitly 

singularity-free. 

Typically, the singularity-free path programming with the minimum cost function is a 

constrained optimization problem. A suitable path should be selected so that the considered 

cost function subject to the conditions and constraints, Eqs. (21)-(25), is minimized. 

3.2 Geometric path representation 

A smooth spatial path can be generated by control points of path function. Because the B- 
spline function provides a simple method to create curves between the defined points, 
therefore, it is used for the path function. The trajectory x at each time sub-interval 
t : <t< t jtl (i=0, 1,. . .,n-l) can be interpolated as 

x{t(u)} = IfiVX,. (26) 

where t(u) = t i + ma( , < u < 1 

M = (f f -u)/n 

U = [u* u 2 u 1 l] 

and x T k (k=-l, 0, ..., n, n+1) are called the control points and total (n+3) points, u is the 

independent spline parameter, N defines the shape of spline, being expressed for the cubic 
B-spline as 
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The associated velocities and accelerations along the trajectory are obtained by subsequent 
derivatives of x as 

x{t(u)}=U'NX i /At (27) 

x{t(u)} = U"NX i /(At) 1 , t { < t < t M (28) 

where the prime is denoted as the derivative with respect to the slpine parameter u. Also, 
from the boundary condition, Eq. (21), the first three control points and the last three are 
fixed as 

*-i = *o = *i = *o ( 29 ) 

*»-l = *,i = *»+l = X f (30) 

Thus, the undetermined control points are reduced to x 2 ,..., x n _ 2 . By substituting the terms 
x, x and x into Eq. (13), then into the cost function as well as the constraint Eqs. (22)-(25), 
the constraint optimization problem can be recast into an unconstrained problem by adding 
weighted penalty functions to form a pseudo cost function, the pseudo cost function can be 
expressed as 

G=Y,At\lF{u,X,)iu+i.{w 1 TR.UE{l . -I [x,u))+ wJRUEQ [x,u)-l ) 

,-0 j,l 

+ w 3 TRUE{\i jmax (x,u)\ - /„„„)) + wJRUE<$ jmKC {x,u)\ - /„„„) + w 5 TRUE(\f pinn (x,u)\ - /„,„)} (31) 

in which the kernel function F(u,X ; ) is associated with the cost function. In the chapter, 

6 I I I r'l 

F=X \fi / F = 1 and F = \f l\ are respectively for the minimum actuating force problem, the 

time optimal problem, and the energy efficiency problem. The constraints are also mapped 
into the function of the spline parameter u and the set of control points x k (k—1, 0, ..., n, 
n+1). In addition, TRUE is a logical operational function with the value one if the statement 
is true and zero if the statement is false; w 1 ,...,W s are the weighting factors that determine 
the magnitude of the penalty and should be taken large enough so that any constraint 
violation will be penalized with large cost. 

By the transformation of the constrained optimization problem, a singularity-free path 
programming is equivalent to the determination of a set of control points for the 
minimization of the pseudo cost function. Although sufficient control points can generate 
any smooth spatial curve, the required computational load for the optimization will be 
increased dramatically. 
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3.3 Global search based on PSOA 

In order to solve the highly nonlinear optimization problem described by the pseudo cost 
function, an evolutionary computation based optimization technique, PSOA mimicking the 
food-searching behavior of birds is employed (Kennedy & Eberhart, 1995). 
The process of PSOA is described as following: 

(1) Randomly initialize position X = [ x T 2 ,..., x 1 _ 2 ] , namely, undetermined control 

points, and velocity V = [ v T 2 ,..., i) T „_ 2 ] , p—1,.. .,P, in which P=40 particles is set. 

(2) Formulate the actuating forces through solving the inverse dynamics Eq. (13), and then a 
numerical integral based on the Simpson's rule is used to calculate the integral of the 

pseudo cost function G,, in Eq. (31) for each particle. 

(3) Compare the current fitness value G,, with the particle's previous best value G,* , if 
G p < G P b i then Pb = X , p=l,. . ., P, in which pb = [pb p2 ■ -pbl „_ 2 f is the position of each 
particle with its personal best fitness value G p . 

(4) Compare the current fitness value G,, with the group's previous best value G s i, , if 
G p < G S b , then gb = X , p=l,..., P, in which gb = [gb 2 ...gb^_ 2 f is the only one that has the 

global best fitness value G S b thus far. 

(5) Update each particle according to PSOA, 

v vi= v P i +c i mnd ( )(p f ' w - f ra ) + c 2 Rand( )(gfe. -x pj ) , )=!,..., 6*(n-3) (32) 
x pj =x p] +v pj (33) 

where v ■ is the rate of the position change for particle p with respect to the jth dimension 
and clamped to the range [-c mnv ,E>„ m ] with u mix =0.8 to prevent the likelihood of the particle 
from leaving the search space; c 1 and c 2 are two positive constants, in which c 1 = c 2 =1.5 are 
set to control the step size that a particle will move in the direction of best position; rand( ) 
and Rand( ) are two normal distribution functions, x ■ is the position of the pth particle with 

respect to the jth dimension. 

(6) If a convergence criterion is met, it means that all the particles have been toward the 
global best and its own individual best. Otherwise, repeat the steps starting from step (2) 
until it has reached the maximum allowable iteration number. The maximum iteration 
number is 350. Furthermore, the relative change in the group's best pseudo objective 

function G S b for two consecutive iterations being less than 0.01 is defined as the convergence 
criterion for the optimization process. 

4. Numerical Simulations 

4.1 Kinematic Parameters 

The optimal singularity-free path programming with the minimum actuating force for the 
parallel robot manipulator is implemented in MATLAB routines. The kinematic parameters 
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used for the simulations are given below, 
The platform points in the B-frame: 
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The base points in the N-frame: 



[A A 2 



*]= 



0.6 0.1 -0.3 -0.3 0.2 0.5 
0.2 0.5 0.3 -0.4 -0.3 -0.2 
0.0 0.1 0.0 0.0 -0.05 0.0 



All legs are considered to be identical. The masses of lower segment 1 and upper segment 2 
of each leg are respective m a = 2kg and m i2 = 1kg, and the mass of the moving platform is 

m B - 32kg. The unit vector s { expressed in the i, -frame is given by s ( = [1 0] . The 
gravity centers of part 1 and part 2 of each leg d n = d l2 = [0.05 0.001 0.001] T m. The 
constant length of the upper segment 2 is l a =1.0 m. The moments of inertia of these two 
segments of each leg as well as the moving platform about their respective gravity centers in 
local frame are % = diag [0.001, 1, 1] kg • m 2 , c I a = diag [0.0001, 0.4, 0.4] kg • m 2 , 
I B = diag [2, 2, 4] kg ■ m 2 . The moments of inertia of the segments 1 and 2 of each leg about 
the respective joint points A;, and A a in local frame can be calculated using parallel axes 
theorem as l n = I {1 + ttl tl d n d n , and l i2 = c l i2 + m j2 a i2 d i2 . The vector of gravity is 
g = [0 -9.81] 1 m/sec 2 . 

The Euler angles vector y =\y/ 9 0] T is used to specify the orientation of the platform and 
each leg, which means that the inertial frame is rotated about the Z axis with y/ radians 
first, resulting in the primed frame; then about the y' axis with 6 radians, resulting in the 
double primed frame; and finally about the x" axis with <j> radians, resulting in the local 
frame. Therefore, the rotation matrix of the local frame relative to the inertial frame is 
written by 



R 



cy/c9 —sy/cg> + cy/s0sg> sy/S(j) + cy/s0c</> 
sy/c6 cy/c(/> + sy/s6stf> —cy/stf> + sy/sdctf, 
—s0 cOs(j> 



(34) 



where c(«) is a shorthand notation for «*?(•) and s(') for sin(') . The velocity transformation 
matrix C L converting the angular velocity in terms of the time derivative of the Euler angles 
to the one with respect to a body-fixed frame is given as 
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—S(f> 






(35) 



where the subscripts e {B,i(i = 1,...,6)} . 

The path planning algorithm presented in the above steps is demonstrated by the following 

simulations. The desired initial pose x a and final pose x f for the parallel robot manipulator 



: [l.6 -0.2 -0.6 0.0 0.0 O.o] , X f = [-0.8 0.1 -0.4 0.0 0.0 0.of 



The straight path connecting these two end-poses and keeping constant orientation is 
through a singular manifold as is shown by the zero determinant of the Jacobian in Fig 3. 
Then, optimal singularity-free paths will be sought between x and x, during the time 

interval subject to each leg's length constraints, l mjtl = 0.2m , l msx = 3.0m , l ma = 0.8m/sec , 

'(„ m =l-0m/sec 2 and /„„„ =2500N. 




Path Parameter 



Fig. 3. Determinant in straight path under constant orientation 

Also, the weighting factors Wi,...,W^ serving as the magnitude of the penalty should be 
chosen appropriately only when the constraints are violated; because a small value may 
yield major constraint violations, but a large value will result in a very poor optimization 
problem. Thus, Wi,w 2 and w 3 are taken as 5xl0 5 and w i = w. = 5xl0 4 by a few trial 

simulations. 

Finally, singularity-free paths are programmed by eight control points, i.e., two 
undetermined control points among them. More control points could imply a more flexible 
search for the optimal singularity-free paths, but with increased control points, it will cost 
more computation time for convergence. 



4.2 Results and Discussions 

According to the aforementioned parameters, the following examples will be illustrated to 
determine the optimal singularity -free paths : 



On the Optimal Singularity-Free Trajectory Planning of Parallel Robot Manipulators 



471 



(l)Minimum actuating force 

The paths programmed for the minimum actuating forces considering the constant 
orientations and varied orientations of the moving platform during the time interval 
t, =20sec are shown in Fig. 4. Fig. 5 is the orientation variations of the moving platform 

while traveling along the planned path with varied orientations. It is seen that these two 
planned paths are quite distinct. 



straight path 

path with constant orientatii 

path with varied orientation 




Fig. 4. Singularity-free planned paths for minimum actuating forces 
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Fig. 5. Orientation of moving platform along plaffhSd path with varied orientations 



Fig. 6 depicts the variations of the determinant along the corresponding paths. As is shown, 
the determinants are never equal to zero for the paths, the obtained paths based on PSOA 
successfully avoid singularities. Moreover, the difference of the evaluated values along the 
two obtained paths is of one order. The result is reflected on the time history plot of each 
leg's actuating forces as shown in Figs. 7(a), (b). It is observed that the actuating forces for 
the actuators 2 and 3 reach their largest values at t—6 sec along the path with a constant 
orientation, this implies that the robot manipulator is the nearest to a singular manifold for 
the pose. 
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Although the actuating forces can be decreased by varying the orientations of the moving 
platform, the required maximum leg lengths increase as indicated in Figs. 8(a), (b). It means 
that a larger task space is necessary to accommodate the planned singularity-free path. 
(2) Time optimum 
For this problem, the travel time t , is to be determined. Based on the singularity-free path 

planning algorithm, the planned trajectory is shown in Fig. 9 (i.e. the line for }i =1) with the 

corresponding minimal travel time t , =5.85 sec. 
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Fig. 6. Variations of determinant along corresponding planned paths 




Fig. 7. Actuating forces along planned paths with (a) constant orientation and (b) varied 
orientations 



(3) Energy efficiency 

Fig. 9 also shows the minimal-energy trajectory with the corresponding travel time i ':, =20.01 

sec (i.e. the line for^i =0). Compared with the time optimal trajectory planning, reduction in 

the travel time is at the expense of a greater consumed energy, a poorer fitness value, and a 
larger force. 

(4) Mixed cost function 
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The cost function is defined as 



G= 1 ikiAt) + (l- F )l'Afi\dt 



(36) 



The optimal singular free trajectories for j,i =0.3, 0.6 and 0.8 with the corresponding 
determined travel time t, =7.758, 6.083 and 6.075 sec are also respectively shown in Fig. 9. 
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Fig. 8. Leg lengths along planned paths with (a) constant orientation and (b) varied 
orientations 



5. Conclusions 

In this chapter, a numerical technique is presented to determine the singularity-free 
trajectories of a parallel robot manipulator. The required closed-form dynamic equations for 
the parallel manipulator with a completely general architecture and inertia distribution are 
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developed systematically using the new structured Boltzmann-Hamel-d'Alembert 
approach, and some fundamental structural properties of the dynamics of parallel 
manipulators are validated in a straight proof. 

In order to plan a singularity-free trajectory subject to some kinematic and dynamic 
constraints, the parametric path representation is used to convert a planned trajectory into 
the determination of a set of undetermined control points in the workspace. With a highly 
nonlinear expression for the constrained optimal problem, the PSOA needing no 
differentiation is applied to solve for the optimal control points, and then the corresponding 
trajectories are generated. The numerical results have confirmed that the obtained 
singularity-free trajectories are feasible for the minimum actuating force problem, time 
optimal problem, energy efficient problem and mixed optimization problem. The generic 
nature of the solution strategy presented in this chapter makes it suitable for the trajectory 
planning of many other configurations in the parallel robot manipulator domain and 
suggests its viability as a problem solver for optimization problems in a wide variety of 
research and application fields. 




Fig. 9. Planned paths for time optimum, energy efficiency and mixed cost function 
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Appendix 

■ Dynamic equation of general parallel robot manipulators 

M x + D x + G =/ (Al) 

where M p = C T l (M l + JlMJJC, = C^C, 
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D„ =C 1 T ((M 1+ / 1 T MJ 1 )C 1 H-(D 1+ / I r DJ I+ /[MJ 1 )C 1 ) =(%((M l +JZMJ 1 )C 1 +D p C 1 ) 

G P =C[(G I+ /[G 2 ) 

f p =Cl(Jacob) T f 

The actuating forces vector /= \f 1 ... fA 
In (Al), the velocity transformation matrix, C 1 , is defined as 



C,= 



C„ 



(A2) 



where C B is the angular velocity transformation of the moving platform. In addition, ] 1 
and J 2 are the sub-matrices appropriately partitioned while developing the equations of 
motion, and are defined as 



/ = [h\hh 



( N 6 Rs 6 ) T ( N 6 Rs 6 ) TN B Rb: 
11 

£ N I? T Co N l? T \ N l?l, T 

-s lt R (-«! ,R ) B R6, 

'i 'i 



11 

6 6 



(A3) 



in which I imi is an n x n unitary matrix such that J 2 = —I 2ix2i ■ 
m Inertia matrix, Coriolis and centrifugal terms, gravity vector 



M, 



M H 

M,. 



M„ 



M 33 M„ 



(A4) 



M 22 =J B 

M,,=l8ag[in u ,...,Jrt (Q ] 

M l3 =diag[m l2 d u s, ,..., m 62 J H s 6 ] 
M u =diag[l 1 ,..., I,] 

and l i =l il +l n -ni il lf i sl +m j2 lAs T i d il +d T il s i \, ;' = !,. ..,6 
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D, 



ro o " 






/D, = 


L° D 22_ 





D„ 



(A5) 



where D 22 = I B a B 



D 31 = diag[m u a T 1 s 1 (l u s 1 +d 12 ) ,..., m 62 G> 6 r s 6 (7 61 s 6 + d 62 )~] 
D 43 = diag^m^sl + dl t )s 1 1 ,..., m 62 (l a s T 6 + d 6 T 2 )s 6 <a 6 ] 
D ii =diag[h l ,..., \\ 



and fej =m i2 / il (/ il s, T +d i2 T )*i + (^i +^2) a, i + m i2'ii( w ;^i2)S; > 



/ = !,.. .,6 



The tildes over the matrix-vector products in D 22 and h, denote a skew-symmetric matrix 
formed from the matrix-vector product. 



(A6) 



where G n = -m B g 



G 31 =[-m 12 g r ^ Sl ... -m 62 g TN 6 Rs 6 ] T 
G*i = [-g T i^KAi + *hhA + mjl^f 



-g T ~R(m 61 d 61 + mj^ij + m 62 d 62 ) T ] 
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1. Introduction 

Programming-by-Demonstration (PbD) is a central research topic in robotics since it is an im- 
portant part of human-robot interaction. A key scientific challenge in PbD is to make robots 
capable of imitating a human. PbD means to instruct a robot how to perform a novel task by 
observing a human demonstrator performing it. Current research has demonstrated that PbD 
is a promising approach for effective task learning which greatly simplifies the programming 
process (Calinon et al., 2007), (Pardowitz et al., 2007), (Skoglund et al., 2007) and (Takamatsu 
et al., 2007). In this chapter a method for imitation learning is presented, based on fuzzy mod- 
eling and a next-state-planner in a PbD framework. For recent and comprehensive overviews 
of PbD, (also called "Imitation Learning" or "Learning from Demostration") see (Argall et al., 
2009), (Billard et al., 2008) or (Bandera et al, 2007). 

What might occur as a straightforward idea to copy human motion trajectories using a simple 
teaching-playback method, it turns out to be unrealistic for several reasons. As pointed out 
by Nehaniv & Dautenhahn (2002), there is significant difference in morphology between body 
of the robot and the robot, in imitation learning known as the correspondence problem. Fur- 
ther complicating the picture, the initial location of the human demonstrator and the robot in 
relation to task (i.e., object) might force the robot, into unreachable sections of the workspace 
or singular arm configurations. Moreover, in a grasping scenario it will not be possible to 
reproduce the motions of the human hand since there so far do not exist any robotic hand 
that can match the human hand in terms of functionality and sensing. In this chapter we will 
demonstrate that the robot can generate an appropriate reaching motion towards the target 
object, provided that a robotic hand with autonomous grasping capabilities is used to execute 
the grasp. 

In the approach we present here the robot observes a human first demonstrating the envi- 
ronment of the task (i.e., objects of interest) and the the actual task. This knowledge, i.e., 
grasp-related object properties, hand-object relational trajectories, and coordination of reach- 
and-grasp motions is encoded and generalized in terms of hand-state space trajectories. The 
hand-state components are defined such that they are invariant with respect to perception, 
and includes the mapping between the human and robot hand, i.e., the correspondence. To 
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enable a next-state-planner (NSP) to perform reaching motions from an arbitrary robot config- 
uration to the target object, the hand-state representation of the task is then embedded into 
the planner. 

An NSP plans only one step ahead from its current state, which contrasts to traditional robotic 
approaches where the entire trajectory is planned in advance. In this chapter we use the term 
"next-state-planner", as defined by Shadmehr & Wise (2005), for two reasons. Firstly, because 
it emphasizes on the next-state planning ability, the alternative term being "dynamic sys- 
tem" which is very general. Secondly, the NSP also act as a controller which is an appropriate 
name, but "next-state-planner" is chosen because the controller in the context of an industrial 
manipulator refers to the low level control system. In this chapter the term planner is more 
appropriate. Ijspeert et al. (2002) were one of the first researchers to use an NSP approach in 
imitation learning. A humanoid robot learned to imitate a demonstrated motion pattern by 
encoding the trajectory in an autonomous dynamical system with internal dynamic variables 
that shaped a "landscape" used for both point attractors and limit cycle attractors. To address 
the above mention problem of singular arm configurations Hersch & Billard (2008) consid- 
ered a combined controller with two controllers running in parallel; one controller acts in 
joint space, while the other one acts in Cartesian space. This was applied in a reaching task for 
controlling a humanoid's reaching motion, where the robot performed smooth motion while 
avoiding configurations near the joint limits. In a reaching while avoiding an obstacle task, 
Iossifidis & Schoner (2006) used attractor dynamics, where the target object acts as a point 
attractor on the end effctor. Both the end-effector and the redundant elbow joint avoided the 
obstacle as the arm reaches for an object. 

The way to combine the demonstrated path with the robots own plan distinguishes our use 
of the NSP from from previous work (Hersch & Billard, 2008), (Ijspeert et al., 2002) and (Ios- 
sifidis & Schoner, 2006). Another difference is the use of the hand state space for PbD; most 
approaches for motion planning in the literature uses joint space (Ijspeert et al., 2002) while 
some other approaches use the Cartesian space. 

To illustrate the approach we describe two scenarios where human demonstrations of goal- 
directed reach-to-grasp motions are reproduced by a robot. Specifically, the generation of 
reaching and grasping motions in pick-and-place tasks is addressed as well as the ability to 
learn from self observation. In the experiments we test how well the skills perform the demon- 
strated task, how well they generalize over the workspace and how skills can be adapted from 
self execution. The contributions of the work are as follows: 

1. We introduce a novel next-state-planner based on a fuzzy modeling approach to encode 
human and robot trajectories. 

2. We apply the hand-state concept (Oztop & Arbib, 2002) to encode motions in hand-state 
trajectories and apply this in PbD. 

3. The combination of the NSP and the hand-state approach provides a tool to address the 
correspondence problem resulting from the different morphology of the human and the 
robot. The experiments shows how the robot can generalize and use the demonstration 
despite the fundamentally difference in morphology. 

4. We present a performance metric for the NSP, which enables the robot to evaluate its 
performance and to adapt its actions to fit its own morphology instead of following the 
demonstration. 



Programming-by-Demonstration of Reaching Motions using a Next-State-Planner 481 

2. Learning from Human Demonstration 

In PbD the idea is that the robot programmer (here called demonstrator) shows the robot 
what to do and from this demonstration an executable robot program is created. We assume 
the demonstrator to be aware of the particular restrictions of the robot. Given this assumption, 
the demonstrator shows the task by performing it in a way that seems to be feasible for the 
robot. In this work the approach is entirely based on proprioceptive information, i.e., we 
consider only the body language of the demonstrator. Furthermore, interpretation of human 
demonstrations is done under two assumptions: firstly, the type of tasks and grasps that can 
be demonstrated are a priori known by the robot; secondly, we consider only demonstrations 
of power grasps (e.g., cylindrical and spherical grasps) which can be mapped to-and executed 
by-the robotic hand. 

2.1 Interpretation of Demonstrations in Hand-State Space 

To create the associations between human and robot reaching /grasping we employ the hand- 
state hypothesis from the Mirror Neuron System (MNS) model of (Oztop & Arbib, 2002). The 
aim is to resemble the functionality of the MNS to enable a robot to interpret human goal- 
directed reaching motions in the same way as its own motions. Following the ideas behind 
the MNS-model, both human and robot motions are represented in hand-state space. A hand- 
state trajectory encodes a goal-directed motion of the hand during reaching and grasping. 
Thus, the hand-state space is common for the demonstrator and the robot and preserves the 
necessary execution information. Hence, a particular demonstration can be converted into the 
corresponding robot trajectory and experience from multiple demonstrations is used to con- 
trol/improve the execution of new skills. Thus, when the robot execute the encoded hand- 
state trajectory of a reach and grasp motion, it has to move its own end-effector so that it 
follows a hand-state trajectory similar to the demonstrated one. If such a motion is success- 
fully executed by the robot, a new robot skill is acquired. 

Seen from a robot perspective, human demonstrations are interpreted as follows. If hand 
motions with respect to a potential target object are associated with a particular grasp type 
denoted G„ it is assumed that there must be a target object that matches this observed grasp 
type. In other words, the object has certain grasp-related features which affords this particular 
type of grasp (Oztop & Arbib, 2002). The position of the object can either be retrieved by a 
vision system, or it can be estimated from the grasp type and the hand pose, given some other 
motion capturing device (e.g., magnetic trackers). A subset of suitable object affordances is 
identified a priori and learned from a set of training data for each grasp type G,. In this way, 
the robot can associate observed grasp types G, with their respective affordances A,. 
According to Oztop & Arbib (2002), the hand-state must contain components describing both 
the hand configuration and its spatial relation with respect to the affordances of the target 
object. Thus, the general definition of the hand-state is in the form: 

H={h 1 ,h 2 ,...h k _ 1 ,h k ,...h p } (1) 

where h\ . . . %_j are hand-specific components which describe the motion of the fingers during 
a reach-to-grasp motion. The remaining components hj c ...hp describe the motion of the hand 
in relation to the target object. Thus, a hand-state trajectory contains a record of both the 
reaching and the grasping motions as well as their synchronization in time and space. 
The hand-state representation in Eqn. 1 is invariant with respect to the actual location and 
orientation of the target object. Thus, demonstrations of object-reaching motions at different 
locations and initial conditions can be represented in a common domain. This is both the 
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strength and weakness of the hand-state approach. Since the origin of the hand-state space 
is in the target object, a displacement of the object will not affect the hand-state trajectory. 
However, when an object is firmly grasped then, the hand-state become fixated and will not 
capture a motion relative to the base coordinate system. This implies that for object handling 
and manipulation the use of a single hand-state trajectory is insufficient. 

2.2 Skill Encoding Using Fuzzy Modeling 

Once the hand-state trajectory of the demonstrator is determined, it has to be modeled for 
several reasons. Five important and desirable properties for encoding movements have been 
identified, and Ijspeert et al. (2001) enumerates them as follows: 

1. The representation and learning of a goal trajectory should be simple. 

2. The representation should be compact (preferably parameterized). 

3. The representation should be reusable for similar settings without a new time consum- 
ing learning process. 

4. For recognition purpose, it should be easy to categorize the movement. 

5. The representation should be able to act in a dynamic environment and be robust to 
perturbations. 

A number of methods for encoding human motions have previously been proposed includ- 
ing splines (Ude, 1993); Hidden Markov Models (HMM) (Billard et al, 2004); HMM com- 
bined with Non-Uniform Rational B-Splines (Aleotti & Caselli, 2006); Gaussian Mixture Mod- 
els (Calinon et al., 2007); dynamical systems with a set of Gaussian kernel functions (Ijspeert 
et al., 2001). The method we propose is based on fuzzy logic which deals with the above 
properties in a sufficient manner (Palm et al., 2009). 

Let us examine the properties of fuzzy modeling with respect to the above enumerated de- 
sired properties. Fuzzy modeling is simple to use for trajectory learning and is a compact 
representation in form of a set of weights, gains and offsets (i.e., they fulfill property 1 and 
2) (Palm & Iliev, 2006). To change a learned trajectory into a new one for a similar task with 
preserved characteristics of a motion, we proposed a modification of the fuzzy time modeling 
algorithm (Iliev et al., 2007), thus addressing property 3. The method also satisfies property 4, 
as it was successfully used for grasp recognition by (Palm et al., 2009). In (Skoglund, Iliev & 
Palm, 2009) we show that our fuzzy modeling based NSP is robust to short perturbations, like 
NSPs in general are known to be robust to perturbations (Ijspeert et al., 2002) and (Hersch & 
Billard, 2008). 

Here it follows a description of the fuzzy time modeling algorithm for motion trajectories. 
Takagi and Sugeno proposed a structure for fuzzy modeling of input-output data of dynami- 
cal systems (Takagi & Sugeno, 1985). Let X be the input data set and Y be the output data set 
of the system with their elements x £ X and y £ Y. The fuzzy model is composed of a set of c 
rules R from which rule R, reads: 

Rule i : IF x IS X,- THEN y = A,x + B, (2) 

X, denotes the z':th fuzzy region in the fuzzy state space. Each fuzzy region X, is defuzzified 
by a fuzzy set J w Xj {x)\x of a standard triangular, trapezoidal, or bell-shaped type. W, £ X, 
denotes the fuzzy value that x takes in the z':th fuzzy region X;. A, and B, are fixed parameters 
of the local linear equation on the right hand side of Eqn. 2. 
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The variable Wj(x) is also called degree of membership of x in X,. The output from rule i is 
then computed by: 

y = w i {x)(A i x + B i ) (3) 

A composition of all rules R\...R C results in a summation over all outputs from Eqn. 3: 



Y^Wi(x)(AiX + Bi) 



(4) 



;'=l 



where wi{x) G [0,1] and Y%=\ w i( x ) = !• 

The fuzzy region X, and the membership function iy, can be determined in advance by design 
or by an appropriate clustering method for the input-output data. In our case we used a 
clustering method to cope with the different nonlinear characteristics of input-output data- 
sets (see (Gustafson & Kessel, 1979) and (Palm & Stutz, 2003)). For more details about fuzzy 
systems see (Palm et al., 1997). 

In order to model time dependent trajectories x(t) using fuzzy modeling, the time instants t 
take the place of the input variable and the corresponding points x(t) in the state space become 
the outputs of the model. 
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Fig. 1. Time-clustering principle. 

The Takagi-Sugeno (TS) fuzzy model is constructed from captured data from the end-effector 
trajectory described by the nonlinear function: 



x(f)=f(f) 

where x(f) G R 3 ,f G R 3 , and t G R+. 

Equation (5) is linearized at selected time points f; with 



x(t) 



, N Af (t) , , 

X(t,)+ At \*> '(*'** 



(5) 



(6) 



resulting in a locally linear equation in t. 



Af(f) 



x(t) = Ai-t + di 

Af(f) 



(7) 



where A, = "tV ; | f, G R 3 and d,- = x(f;) Af If, ' h ^ ^ '• Using Eqn. 7 as a local linear 

model one can express Eqn. 5 in terms of an interpolation between several local linear models 
by applying Takagi-Sugeno fuzzy modeling (Takagi & Sugeno, 1985) (see Fig. 1) 
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x(t) = £w,(t).(Ai-f+di) (8) 

1=1 

Wj (t ) 6 [0, 1] is the degree of membership of the time point t to a cluster with the cluster center 

tj, c is number of clusters, and £jLi W;(f) = 1. 

The degree of membership zf,(£) of an input data point t to an input cluster Q is determined 

by 

1 

W i{t) = - r 1 ( 9 ) 

.i-y (t-tjVMj (t-tj i 

j — 1 v ;/ / pro v / 

The projected cluster centers f, and the induced matrices M, „ ro define the input clusters C, (i = 
1 . . . c). The parameter m pro > 1 determines the fuzziness of an individual cluster (Gustafson 
& Kessel, 1979). 

2.3 Model Selection using Q-learning 

The actions of the robot have to be evaluated to enable the robot to improve its performance. 
Therefore, we use the state-action value concept from reinforcement learning to evaluate each 
action (skill) in a point of the state space (joint configuration). The objective is to assign a 
metric to each skill to determine its performance, and include this is a reinforcement learning 
framework. 

In contrast to most other reinforcement learning applications, we only deal with one state- 
action transition, meaning that from a given position only one action is performed and then 
judged upon. A further distinction to classical reinforcement learning is to ensure that all 
actions initially are taken so that all existing state-action transitions are tested. Further im- 
provement after the initial learning and adaption is possible by implementing a continuous 
learning scheme. Then, the robot will receive the reward after each action and continues to im- 
prove the performance over a longer time. However, this is beyond the scope of this chapter, 
and is a topic of future work. 
The trajectory executed by the robot is evaluated based on three criteria: 

• Deviation between the demonstrated and executed trajectories. 

• Smoothness of the motion, less jerk is preferred. 

• Successful or unsuccessful grasp. 

In a Q-learning framework, the reward function can be formulated as: 

1 t=tf 1 t=tf 2 

r = --'£\H*(t)-H(t)\--'£ x 2 (t)+r g (10) 

l f f=0 V f=0 



where 



!— 100 if Failure 
if Success, but overshoot (11) 

+100 if Success 

where H r (t) is the hand-state trajectory of the robot, H(t) is the hand-state of the demon- 
stration. The second term of the Eqn. 10 is proportional to the jerk of the motion, where tt 
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is the duration of the motion, fg is the starting time and x is the third derivative of the mo- 
tion, i.e., jerk. The third term in Eqn. 10 fv, is the reward from grasping as defined in Eqn. 11 
where "failure" meaning a failed grasp and "success" means that the object was successfully 
grasped. In some cases the end-effector performs a successful grasp but with a slight over- 
shoot, which is an undesired property since it might displace the target. An overshoot means 
that the target is sightly missed, but the end-effector then returns to the target. 
When the robot has executed the trajectories and received the subsequent rewards and the 
accumulated rewards, it determines what models to employ, i.e., action selection. The actions 
that received the highest positive reward are re-modeled, but this time as robot trajectories 
using the hand-state trajectory of the robot as input to the learning algorithm. This will give 
less discrepancies between the modeled and the executed trajectory, thus resulting in a higher 
reward. In Q-learning a value is assigned for each state-action pair by the rule: 

Q(s,a) = Q(s,a) + a*r (12) 

where s is the state, in our case the joint angles of the manipulator, a is the action, i.e., each 
model from the demonstration, and a is a step size parameter. The reason for using the joint 
space as state space is the highly non-linear relationship between joint space and hand-state 
space (i.e., a Cartesian space): two neighboring points in joint space are neighboring in Carte- 
sian space but not the other way around. This means that action selection is better done in 
joint space since the same action is more likely to be suitable for two neighboring points than 
in hand-state space. 

To approximate the Q-function we used Locally Weighted Projection Regression (LWRP) as 
suggested in (Vijayakumar et al., 2005), see their paper for details. 

3. Generation and Execution of Robotic Trajectories based on Human Demonstra- 
tion 

This section covers generation and execution of trajectories on the actual robot manipulator. 
We start with a description of how we achieve the mapping from human to robot hand and 
how to define the hand-state components. Then, section 3.3 describes the next-state-planner, 
which produces the actual robot trajectories. 

3.1 Mapping between Human and Robot Hand States 

The definition of H is perception invariant and must be able to update from any type of sensory 
information. The hand-sate components hy . . . h„ are such that they can be recovered both 
from human demonstration and from robot perception. Fig. 2 shows the definition of the 
hand-state in this article. 

Let the human hand be at some initial state H\ . The hand then moves along the demonstrated 
path until the final state He is reached where the target object is grasped by the hand (Iliev 
et al., 2007). That is, the recorded motion trajectory can be seen as a sequence of states, i.e., 

H(t) : Hifa) ->■ H 2 (t 2 ) ->...-> H f (t f ) (13) 

Since the hand state representation is defined in relation to the target object, the robot must 
have access to the complete trajectory of hand of the demonstrator. Therefore the hand-state 
trajectory can only be computed during a motion if the target object is known in advance. 



1 Available at: http://wrww-clmc.usc.edu/software/lwpr 
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axis 

Fig. 2. The hand-state describes the relation between the hand pose and the object affordances. 
N ee is the the normal vector, O ee the side (orthogonal) vector and A ee is the approach vector. 
The vector Q ee is the position of the point. The same definition is also valid for boxes, but with 
the restriction that the hand-state frame is completely fixed, it cannot be rotated around the 
symmetry axis. 



Let H,j es (f) be the desired hand-state trajectory recorded from a demonstration. In the general 
case the desired hand-state H^ cs (f) cannot be executed by the robot without modification. 
Hence, a robotic version of H^ es (t) have to be constructed, denoted by H r (t), see Fig. 3 for an 
illustration. 

One advantage of using only one demonstrated trajectory as the desired trajectory over trajec- 
tory averaging (e.g., (Calinon et al., 2007) or (Delson & West, 1996)) is that the average might 
contain two essentially different trajectories (Aleotti & Caselli, 2006). By capturing a human 
demonstration of the task, the synchronization between reach and grasp is also captured, 
demonstrated in (Skoglund et al., 2008). Other ways of capturing the human demonstration, 
such as kinesthetics (Calinon et al., 2007) or by a teach pendant (a joystick), cannot capture 
this synchronization easily. 

To find H r (t) a mapping from the human grasp to the robot grasp a transformation is needed, 
denoted T^. This transformation can be obtained as a static mapping between the pose of 
the demonstrator hand and the robot hand while they are holding the same object at a fixed 
position. Thus, the target state ff, will be derived from the demonstration by mapping the 
goal configuration of the human hand Hr into a goal configuration for the robot hand ffi, 
using the transformation IT: 



H f 



UH f 



(14) 



Since HC 



The pose of the robot hand at the start of a motion defines the initial state HI 
represents the robot hand holding the object , it has to correspond to a stable grasp. For 
a known object, suitable H r f can either be obtained by simulation (Tegin et al., 2009), grasp 
planning or by learning from experimental data. Thus, having a human hand state Hf and 
their corresponding robot hand state ffi, IT is obtained as: 



H f H J l 



(15) 
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It should be noted that this method is only suitable for power grasps. In the general case it 
might produce ambiguous results or rather inaccuarate mappings. 




Fig. 3. Mapping from human hand to robotic gripper. 



3.2 Definition of Hand-States for Specific Robot Hands 

hen the initial state H{ and the target state ffi are defined, we have to generate a trajectory 

between the two states. In principle, it is possible to use Eqn. 14 to H^ es (t) such that it has 
its final state in H r f . The robot then starts at Hj and approaches the displaced demonstrated 
trajectory and then track this desired trajectory until the target state is reached. However, such 
an approach would not take trajectory constraints into account. Thus, it is also necessary to 
specify exactly how to approach H^ es (t) and what segments must be tracked accurately. 
The end-effector trajectory is reconstructed from the recorded demonstration, and is repre- 
sented by a time dependent homogeneous matrix T ee (t). Each element is represented by the 
matrix 



N ee 





O ee 





A-ee 




Qee 
1 



(16) 



where N ee , O ee and A ee are the normal vector, the side vector, and the approach vector, respec- 
tively of the end effector. The position is represented by the vector Q ee . It is important to note 
that the matrix T ee is defined differently for different end-effectors, for example, the human 
hand is defined as in Fig. 2. 

From the demonstrated trajectory, a handstate trajectory can be obtained as afunction of time. 
We formulate the hand-state as: 

H(t) = [d„(t) d {t) d a (t) <p„(t) cp (t) <p a {t)} (17) 

The individual components denote the position and orientation of the end-effector. The first 
three components, d n (t), d (t) and d a (t), describe the distance from the object to the hand 
along the three axes n, o and a with the object as the base frame. The next three components, 
<f>n(t)i <Po(t) and <p a (t), describe the rotation of the hand in relation to the object around the 
three axes n, o and a. The notion of the hand-state used in this section is illustrated in Fig. 2. 



488 Advances in Robot Manipulators 

Note that by omitting the finger specific components of the hand-state we get a simpli- 
fied definition of H, but cannot determine the type of human grasp. In (Skoglund et al., 
2008) we give an account of how the finger specific components and object relation com- 
ponents can be used to synchronize reaching with grasping. Another reason for omitting 
finger specific components is that grasp classification is out of scope of this chapter; only 
power grasps are used the subsequent experiments. Thus, the grasp type is assumed to be 
known G = {cylindrical, spherical, plane}; the affordances are: position, size, and cylinder axis 
A = {width, axis} or box A = {width, length, N-axis, O-axis, A-axis}. See (Palm & Iliev, 2007) 
for grasp taxonomy. 

3.3 Next-State-Planners for Trajectory Generation 

In this section we present the next-state-planner (NSP) that balances its actions between/o/Zoif- 
ing a demonstrated trajectory and approaching the target, first presented in (Skoglund et al., 2008). 
The NSP we use is inspired by the Vector Integration To Endpoint (VITE) planner propsed 
by Bullock & Grossberg (1989) as a model for human control of reaching motions. The NSP- 
approach requires a control policy, i.e., a set of equations describing the next action from the 
current state and some desired behavior. 

The NSP generates a hand-state trajectory using the TS fuzzy-model of a demonstration. Since 
the resulting hand-state trajectory H r (t) can easily be converted into Cartesian space, the in- 
verse kinematics provided by the arm controller can be used directly. 

The TS fuzzy-model serves as a motion primitive for the arm's reaching motion. The initial 
hand-state of the robot is determined from its current configuration and the position and ori- 
entation of the target object, since these are known at the end of the demonstration. Then, 
the desired hand-state H r , is computed from the TS fuzzy time-model (Eqn. 8). The desired 
hand-state H,j fs is fed to the NSP. Instead of using only one goal attractor as in VITE (Bullock 
& Grossberg, 1989), and additional attractor-the desired hand-state trajectory-is used at each 
state. The system has the following dynamics: 

H = u(-H + f}(H g -H)+ 7 (H des -H)) (18) 

where Hg is the hand-state goal, M des the desired state, H is the current hand-state, H and H 
are the velocity and acceleration respectively, a is a positive constant (not to be confused with 
the step size parameter a in Eqn. 12) and /3, 7 are positive weights for the goal and tracking 
point, respectively. 

If the last term 7(H^ es — H) in Eqn. 18 is omitted, i.e., 7 = 0, then the dynamics is exactly as the 
VITE planner Bullock & Grossberg (1989). Indeed, if no demonstration is available the planner 
can still produce a motion if the target is known. Similarly, if the term fi(Hg — H) is omitted, 
the planner becomes a trajectory following controller. To determine /3 and 7, which controls 
the behavior of the NSP, we use a time dependent weighting mechanism. The weighting is a 
function of time left f; to reach the goal at 1 1; 7 = K[ti/tt) , where K is 2; f> = 1 — 7. Since 
the environment demonstration provides better accuracy than the task demonstration, it is 
reasonable to give the target a hight weight at the end of the trajectory (i.e., f> = 1), Spherical 
linear interpolation is used. To interpolate between initial and final orientation along the 
trajectory spherical linear interpolation is used (Shoemake, 1985). 

It is also possible to determine /3 and 7 by the variance across multiple demonstrations 
(Skoglund, Tegin, Iliev & Palm, 2009). 
Analytically, the poles in Eqn. 18 are: 
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P1/P2 



h 



ay. 



(19) 



The real part of f\ and f>2 will be < 0, which will result in a stable system (Levine, 1996). 
Moreover, a. j£ 47 and a > 0, 7 > will contribute to a critically damped system, which is 
fast and has small overshoot. Fig. 4 shows how different values 7 affect the dynamics of the 
planner. 



Tacking of tanh(t), dt=0.01 , a=8, y = [2:64] 




Fig. 4. The dynamics of the planner for six different values of 7. The tracking point is tanh(t), 
with dt = 0.01 and a is fixed at 8. A low value on 7 = 2 produces slow dynamics (black 
dot-dashed line), while a high value 7 = 64 is fast but overshoots the tracking point (black 
dashed line). 



The controller has a feedforward structure as in Fig. 5. The reason for this structure is that 
a commercial manipulator usually has a closed architecture, where the controller is embed- 
ded in the system. For this type of manipulators, a trajectory is usually pre-loaded and then 
executed. Therefore, we generate the trajectories in batch mode for the ABB140 manipulator. 
Since our approach is general, for a given different robot platform with hetroceptive sensors 
(e.g., vision) our method can be implemented in a feedback mode, but this requires that the 
hand-state H(t ) can be measured during execution. 

3.4 Demonstrations of Pick-and-Place Tasks 

In our setup a demonstration is done in two stages: environment- and task demonstration. 
During the first stage, the environment demonstration, the target objects in the workspace are 
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Fig. 5. Hand-state planner architecture. H ? is the desired hand-state goal, H^ es is the desired 
hand-state at the current distance to target. 



shown to the robot. The environment demonstration can provide a more accurate workspace 
model than the task demonstration, since this is a separated stage and the demonstrator can 
focus on the objects and don't consider the dynamics of the task. In the experiment in sec- 
tion 5.2, we use a simplistic modeling of the environment where all objects are modeled as 
box shaped objects. The result from an environment demonstration is shown in Fig. 6, where 
the bounding boxes were created from information on hand/fingerpose and tactile data. A 
bounding box represents each object with position of the center and length, width and height, 
which are used to compute the orientation of the object. A more sophisticated modeling-based 
on point clouds-could be used if a better granularity of the workspace is needed (Charusta 
etal.,2009). 

In the task demonstration, i.e., a pick-and-place of an object, only the task is shown. Once the 
workspace model is available, only the demonstrated trajectory is used. If an environment 
demonstration is unavailable the target object can be determined from task demonstration, 
where the center point of the grasp can be estimated from the grasp type. However, this not as 
accurate as using data form an environment demonstration. The task demonstration contains 
the trajectories which the robot should execute to perform the task. Trajectories recorded from 
a task demonstration are shown in Fig. 7. 

4. Experimental Platform 

For these experiments human demonstrations of a pick-and-place task are recorded with two 
different subjects, using the PhaseSpace Impulse motion capturing system. The Impulse sys- 
tem consists of four cameras (in experiment 1), and five (in Experiment 2) mounted around 
the operator to register the position of the LEDs. Each LED has a unique ID by which it is 
identified. Each camera can process data at 480 Hz and have 12 Mega pixel resolution result- 
ing in sub-millimeter precision. One of the cameras can be seen in the right picture of Fig. 8. 
The operator wears a glove with LEDs attached to it, left picture see Fig. 8. Thus, each point 
on the glove can be associated with a finger, the back of the hand or the wrist. To compute the 
orientation of the wrist, three LEDs must be visible during the motion. The back of the hand is 
the best choice since three LEDs are mounted there and they are most of the time visible to at 
least three cameras. One LED is mounted on each finger tip, and the thumb has one additional 
LED in the proximal joint. One LED is also mounted on the target object. Moreover, we have 
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Fig. 6. The result of an environment demonstration. 



mounted tactile sensors (force sensing resistors) to detect contact with objects. The sensors are 
mounted on the fingertips of the glove, shown in the middle of Fig. 8. We define a grasp as 
when contact is detected at the thumb sensor and one additional finger. This means that only 
grasps which include the thumb and one other finger can be detected. No grasp recognition is 
necessary since the gripper only allows one grasp type. When a grasp is detected the distance 
to each object in the workspace is measured and the nearest object, if below some distance 
threshold, is identified as the target object. 

The motions are automatically segmented into reach and retract motions using the velocity 
profile and distance to the object. The robot used in the experiments is the industrial manipu- 
lator ABB IRB140. In this experiment we use the anthropomorphic gripper KTHand (Fig. 9), 
which can perform power grasps (i.e., cylindrical and spherical grasps) using a hybrid posi- 
tion/force controller. For details on the KTHand, see (Tegin et al., 2008). 

The demonstrations were performed with the teacher standing in front of the robot. First, 
the environment is demonstrated by tactile exploration of the workspace. The demonstrator 
touches the objects of interest; with special care so the boundaries of each object are correctly 
captured. Second, the task is demonstrated where the teacher starts with the hand in a position 
similar the robot's home position, i.e., = [000000]. The results from the environment 
and task demonstrations are shown in Fig. 6 and 7 respectively, with the base frame of the 
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Robot and Wrist trajectory 




y-axis 



Fig. 7. Five sample demonstrated trajectories (the wrist is plotted), the robot and the two 
objets of interest. The object to the right is marked with a red star to indicate that this object 
was the nearest when a grasp was detected in the task demonstration. 




Fig. 8. Left: The glove used in the Impulse motion capturing system from PhaseSpace. The 
glove from the top showing the LEDs. Middle: The glove with the tactile sensors mounted 
on each finger tip. Right: The system in use showing one of the cameras and the LED on the 
glove. 



robot as the reference frame. The object is determined from the environment demonstration, 
since it is more exact compared to the task demonstration. 



5. Experimental Evaluation 

In this section we provide an experimental evaluation of the presented method. 
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Fig. 9. The anthropomorphic gripper KTHand used in the second experiment. 



5.1 Experiment 1 - A Complete Pick-and-Place Task 

To test the approach on an integrated system the KTHand is mounted on the ABB manipulator 
and a pick-and-place task is executed, guided by a demonstration showing pick-and-place 
task of a box (110 x 56 x 72 mm). The synchronization between reach and grasp is performed 
by a simple finite state machine. After the grasp is executed, the motion to the placing point 
is performed by following the demonstrated trajectory (see section 2.2). Since the robot grasp 
pose corresponds approximately to the human grasp pose it is possible for the planner to 
reproduce the human trajectory almost exactly. This does not mean that the robot actually can 
execute the trajectory, due to workspace constraints. The retraction phase follows the same 
strategy as the reaching motion, but in reverse. Fig. 10 shows the complete task learned from 
demonstration. 



t^tu^t^t^ 




Fig. 10. Industrial manipulator programmed using a demonstration. A movie of the sequence 
is available at: http://www.aass.oru.se/Research/Learning/arsd.html. 
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5.2 Experiment 2 - Skill Improvement 

In this experiment the robot uses the self executed trajectories for skill modeling and compare 
the performance to the skills models from human demonstration. The aim is to enable to 
robot to improve its performance. In this experiment 20 task demonstrations of the same pick- 
and-place task were performed, where only the reaching part of the task was used to train a 
reaching skill. Five of these demonstrations are shown in Fig. 7. In addition, one environment 
demonstration was also recorded, shown in Fig. 6. All demonstrations were modeled using 
fuzzy time-modeling, where the position of the index finger and orientation of the wrist were 
used to record the trajectory. Three of the modeled trajectories are shown in figure 11 in the 
top graphs as dashed lines. The reason for using the index finger instead of-what would be 
more appropriate-the center point between the index finger and the tip of the thumb is that 
the LED on the thumb was often hidden resulting from occlusions during the motion. Thus, 
the number of occlusions is minimized. 








Fig. 11. Demonstrations number H(„ H\\ and H20, of which the two former were the two best 
performing models, and the latter was the worst performing model in generating the reaching 
trajectory for the robot to execute. Top: The dashed lines in the top graphs are the modeled 
trajectories from the demonstration H^ ra . The solid lines are the trajectory executed by the 
robot, H r . Bottom: Both as H^ es (dashed) as the human demonstrated in, and H r (solid) as the 
robot executed it, Cartesian space. 



5.2.1 Evaluation of Hand-State Trajectories 

All the recorded demonstrations were modeled and executed by the robot in the same manner 
as in Exp. 1. In addition the performance of each model was evaluated, the criteria in Eqn. 10. 
Three of the executed trajectories are shown in Fig. 11 in the top graph as solid lines and in 
the bottom graph as Cartesian trajectories. At the end of the demonstrations (around 3.5 to 
4.0 sec) in Fig. 11 the difference between the model and the actual trajectory is up to 8 cm (in 
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y-direction). Since the model is created from the trajectory of the fingertip of the index finger, 
the end of the trajectory will be displaced from the center of the target object. This is due to the 
fact that the index finger will be at the side of the box shaped object during the grasp. Recall 
from the definition of the hand-state space (section 2.1) that the target object is the frame in 
which all motions are in relation to. This means that the origin of the hand-state frame is the 
top center of the target object. The point attractor of Eqn. 18 for the trajectory generation is 
switching from the task demonstration to the target object, as described below. Therefore, the 
point [0, 0, 0] becomes the point attractor at the end of the motion. 

The hand-state error and minimum jerk can be evaluated already in the generation state in 
simulation, i.e., before the actual execution on the real robot. The grasp success is eval- 
uated from real execution. In the real execution the robot starts in the home position 
©= [0 0000 0] deg, with a small perturbation added to the last three joints since the 
home position is a singular configuration. All models succeeded in generating a trajectory 
which positioned the end-effector in such a way that a successful grasp can be performed, 
resulting in a positive reward of +100. This reward is then decreased by adding a negative 
reward from hand-state error and jerk, as described by equations 10-11. 
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Table 1. The Q-values at the home configuration, ©= [0 0] T deg. When the robot 
imitated human actions Hg and H\\ (light gray) from its home configuration, it received the 
highest Q-value and used these two to create the two new action models R\ and 7?2- To com- 
pare the two original motions Hg and Hjj were selected since they performed best and H20 
which had the lowest positive Q-value. 

In Table 1 this is shown in the rows with joint configuration 0= [0 0000 0] deg. The mod- 
els from human demonstration Hg and Hy performed best when the robot executed them. 
Hence, these values were selected to be remodeled into robot skills: Ri and R2, meaning that 
the robot trajectory is used to create the fuzzy models. Worst performance was obtained in 
H19 and H20, where Hjg received a negative Q-value. Finally, R^ and R2 were tested from 
the home configuration and evaluated using the same criteria, shown in Fig. 12. As expected, 
their performance was better than all others, since the hand-state error is reduced. 



5.2.2 Generalization of Robot Skills 

Generalization over the workspace is used as a test on how well the skills modeled from robot 
execution perform in comparison to the skills modeled from human demonstration. This 
is done by positioning the manipulator in twelve starting configurations, different from the 
home configuration © = [0 0] T , while keeping the target object at the same position 
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as in the demonstration. The models Hg and Hu, acquired from human demonstrations, re- 
ceived the highest Q-values (see Table 1) and are therefor selected for comparison. In addition 
we also selected one of the worst scoring models of the remaining models, namely H2o- A set 
of twelve joint configurations were chosen as the initial starting positions. The configurations 
were chosen to approximately correspond to 1/4 of the full joint range at each joint, except 
last joint where 1/8 of the range was used. Two exceptions from this were the configurations 
© = [0 20 40 1 1 l] r and @=[0 25 111] T due to workspace restrictions. The resulting Q- 
values after executing a reaching motion form these joint configurations can be seen in Table 2. 
In the table the highest Q-values are highlighted. From two of the starting configurations none 
of the models could execute a successful reaching motion, namely © = [0 — 45 1 1 1] T deg 
and ©=[0 001 — 55 1] deg (dark rows in Table 2). This means that new demonstrations 
should be done from these positions. From the other joint configurations neither Hg nor H\\ 
performed best. Surprisingly, the worst model H20 was the only one to perform a successful 
reaching motion from configuration ©=[0 — 50 111] deg, resulting in the highest Q- 
value. From the rest of the configurations robot skill R\ or R2 performed best, thus confirming 
our hypothesis that skills modeled from own experience are better adapted to the robot than 
skills directly modeled from observation. Fig. 13 shows four sample configurations where the 
robot skill R4 and R2 are used to generate reaching actions from configurations different from 
the trained position. In Fig. 14 two sequences are shown where the real robot executes the 
reaching skills from the trained position, and from the tested position. 
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Table 2. The Q-values for selected joint configurations. Twelve other configurations than the 
home configuration were selected to test how well the robot skills R\ and R2 performed. As 
comparison, the two original motions Hg and H\\ were selected since they performed best. In 
addition H20 was selected, because it had the lowest positive Q-value. 



6. Conclusions and Future Work 

In this article, we present a method for programming-by-demonstration of reaching motions 
which enables robotic grasping. To allow the robot to interpret both the human motions and 
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Fig. 12. Robot model R\ and the generated reaching trajectory the robot executes. 



its own in similar ways, we employ a hand-state space representation as a common basis 
between the human and the robot. 

We presented the design of a NSP, which includes the advantages of fuzzy modeling and 
executes the motion in hand-state space. 
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Robot- and Demonstrated trajectory 21 



Robot- and Demonstrated trajectory 22 




Fig. 13. Reaching from different initial positions. Four sample configurations, © 

[-45 11 l] r , = [0 20 40 1 1 1] T , © = [0 25 1 1 l] r and © = [0 1 55 l] r . 



Human demonstrations have been shown to provide sufficient knowledge to produce skill 

models good enough for the robot to use as its own skills. 

It is shown that the suggested method can generate executable robot trajectories based on 

current and past human demonstrations despite morphological differences. The robot gains 

experience from human demonstration, and we have shown how the robot can improve when 

the selfexecuted motions are performed. The generalization abilities of the trajectory planner 

are illustrated by several experiments where an industrial robot arm executes various reaching 

motions and performs power grasping with a three-fingered hand. 

To validate the performance of the skills learned from demonstration we included a metric 

which can be used in reinforcement learning. The performances of the initially learned skills 

were compared to the skills learned from self execution (robot skills). The result showed that 

robot skills indeed performed better than skills from human demonstration. In reinforcement 

learning the performance metric is used to formulate the reward function. 

In our future work we plan to extend the theoretical and experimental work to include all 

feasible grasp types of the KTHand. To remedy the effect of the small workspace of the robot 
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R 2 from configuration = [0 0] 3 




R 2 from configuration = [0 20 40 1 1 1] 




Fig. 14. The top series show the robot grasping the box shaped object from configuration 
©= [0 0000 0] using robot skill R 2 . The bottom graph show the same skill R 2 but from a 
different initial position than the trained one: = [0 20 40 1 1 1] T . 



a different workspace configuration will be used. Furthermore, the robot's own perception 
will be incorporated into the loop to enable the robot to learn from its own experience. 
Acknowledgment: Johan Tegin at Mechatronics Laboratory, at the Royal Institute of Tech- 
nology, Stockholm, should be acknowledged for providing access to the KTHand. Krzysztof 
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1. Introduction 

The use of industrial robots in production started a rapid expansion in the 1980s, since they 
had the possibility of improving productivity being able to work for extended periods of 
time with good repeatability therefore making the quality of products stable. However, since 
the first robots worked "blindly", on a pre-programmed trajectory, dedicated equipment had 
to be prepared only for supplying the workpieces to the robots (Inaba & Sakakibara, 2009). 
Also, human operators had to manually align the workpieces before the robot was able to 
manipulate them. 

The intelligent robots appeared later in 2001 in order to solve this problem. An intelligent 
industrial robot is not a humanoid robot that walks and talks like a human, but one that 
performs complex tasks, similar to a skilled worker. This is achieved with sensors (vision, 
force, temperature etc) and artificial intelligence techniques. 

Today, solutions to problems like picking parts placed randomly in a bin (bin picking), which 
were considered difficult a few years ago, are now considered mature: (Hardin, 2008) and 
(Iversen, 2006). A similar problem is auto racking, where robots have to pick parts which 
are presented one at a time, although the exact location and 3D orientation varies. These 
applications are made possible using 3D vision sensors. 

1 .1 3D vision sensors 

Two types of vision sensors are used on the factory floor: two-dimensional (2D) and three- 
dimensional (3D). The 2D sensors are usually similar to a digital photographic camera, being 
able to capture an image of the workpiece and obtain the position and rotation angle of the 
object. This works well for parts that can sit on a flat surface, and enables them to be picked 
by a SCARA robot, for example. 
There are two major methods for 3D vision sensors: 

• Structured light 

• Stereoscopic vision 

1.2 Structured light sensors 

Structured light sensors have a common principle: projecting a narrow band of light onto 
a three-dimensionally shaped surface produces a line of illumination that appears distorted 
when viewed from other perspectives than that of the projector. The shape of the line of 
illumination can be captured by a 2D camera, allowing the exact geometric reconstruction of 
the surface shape using the triangulation method. 
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The simplest sensor using the triangulation principle is the range finder , which measures the 
distance to the closest reflective object (Fig. 1). A pulse of light (laser, regular visible light 
or infrared) is emmited and then reflected back on a linear CCD array. The position of the 
reflected light on the CCD array can be used to compute the distance to the closest object. 
This kind of sensor is also affordable to robot hobbists (Palmisano, 2007). 




Fig. 1. Triangulation-based laser range finder 

A more complex sensor is the profile scanner, which projects a narrow stripe of laser light 
onto the surface being digitized. A 2D camera, placed at a known angle with respect to the 
laser plane, records the image of the laser stripe and computes the local geometrical shape of 
the surface. For reconstructing a full 3D model of the workpiece, the profile scanner has to 
be swept around the part. The most precise way is to use a coordinate measuring machine 
(CMM). The sensors can also be mounted on industrial robots, which are more flexible in 
positioning and orienting the sensor, but also less accurate. Laser-based vision systems can 
generate very accurate 3D surface maps of the digitized parts, depending on the quality of the 
components used, but can be slow, since the sensor has to be moved continuously around the 
workpiece. 

A faster method is projecting a pattern consisting of more light stripes. A typical setup for 3D 
measuring has a stripe projector, which is similar to a video projector, and at least one camera. 
Common setups include two cameras in opposite sides of the projector. 

The depth can be reconstructed by analyzing the stripe patterns recorded by the camera, and 
several algorithms are available. The displacement of any single stripe can be converted into 
3D coordinates; this involves identifying the stripes, either by tracing or counting stripes; am- 
biguities may appear when the workpiece contains sharp vertical walls. Another method in- 
volves alternating stripe patterns, resulting in binary sequences. Depth may also be computed 
by variations in the stripe width along the surface, and also by frequency / phase analysis of 
the stripe pattern by means of Fourier or Wavelet transforms. Practical implementation com- 
bine these methods in order to reconstruct a complete and unambiguous model of the surface. 



1.3 Stereoscopic Vision 

Stereoscopic vision attempts to compute the third dimension (the depth) in a way similar to 
the human brain. A 3D binocular stereo vision system uses two cameras which take images 
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of the same scene from different positions (Fig. 2), and then computes the 3D coordinates for 
each pixel by comparing the parallax shifts between the two images. 
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Fig. 2. Stereo vision principle: two cameras, which view the same scene, detect a common 3D 
point on different 2D locations 

The main process in stereoscopic vision is the stereo correspondence between the two images, 
which is used to estimate disparities (differences in image locations of an object recorded by 
the two cameras) Calin & Roda (2007). The disparity is negatively correlated with the dis- 
tance from the cameras: as the distance from the camera increases, the disparity decreases. 
Using geometry and algebra, the points that appear in the 2D stereo images can be mapped 
as coordinates in 3D space. 

The stereo matching problem was, and continues to be, one of the most active research areas in 
computer vision. Several algorithms were developed; a classification and comparative bench- 
mark for dense two-frame correspondence algorithms is presented in (Scharstein & Szeliski, 
2002). 

Aside from depth perception, stereo matching is also used in mobile robotics, where compar- 
ing two succesive images taken with the same camera leads to estimation of the motion of the 
robot. 

For robotic applications, which require 6-DOF part localization in 3D space, stereo vision is 
considered also a mature technology: (Hardin, 2008) and (Iversen, 2006). 3D part localization 
is also possible even with a single 2D camera, using a trained model of the part, and the ap- 
proach is already used in production (Iversen, 2006). An experiment showing the integration 
of a binocular stereo vision system with an industrial robot is presented by Cheng & Chen 
(2008). 

The main advantage of stereo vision techniques is that they do not require additional light 
sources, and therefore, these techniques are non-invasive with respect to the surrounding en- 
vironment. 



2. 3D reconstruction system 

An application involving a 6-DOF robot arm and a profile scanning device is presented in this 
section. The structure of the system can be seen in Fig. 3(a). The main components are: 

• Short range laser probe, able to measure distances between 100 and 200 mm with 30 um 
accuracy, using one laser beam and two CCD cameras; 
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6-DOF vertical robot arm, with 650 mm reach and 20 um repeatability; 
1-DOF rotary table, for holding the workpiece being scanned; 
4-axis CNC milling machine, for reproduction of the scanned parts. 




1 ) PC - CNC communication (RS232) 

2) PC - robot controller communication (TCP/IP) 

3) Electrical connection between robot arm and robot controller 

4) PC - Laser probe communication (USB) 

5) Trigger signal from the laser probe (RS485 Digital I/O) 

6) Robot controller - Rotary table controller communication (Digital I/O and RS232) 

7) Electrical connection between rotary table controller and rotary table 
mechanical subsystem (DC motor and optical encoder) 



Fig. 3. (a) Overview of the 3D scanning system; (b) Laser sensor scanning a dark surface 



2.1 Simulation platform 

Before installing the physical hardware, a software simulation platform was developed in 
order to test the scanning strategies, develop the motion planning algorithms and analyze the 
laser sensor behavior in controlled situations. 
The simulator, presented in detail in (Borangiu et al., 2008a), has two components: 

• Robot motion simulation, which uses a 7-DOF kinematic model based on Denavit- 
Hartenberg convention (Spong et al., 2005) and renders individual rigid meshes for 
each DOF of the system; 

• Optical simulation of the laser sensor in a virtual 3D world, using raytracing. 

The simulator has two modes of operation: static and dynamic simulation. In the static mode, 
the robot maintains the position of the laser sensor fixed, and the two CCD image sensors 
show a simulated image. In dynamic mode, the user can specify complex scanning trajectories 
which will be followed. The result from the laser sensor is analyzed and a point cloud model, 
representing the scanned virtual part, is created. The simulator is also capable of exporting 
animations with the scanning system following a predefined program. 

The profile scanner emits a laser beam focused into a plane, which, projected on a surface, is 
reflected on the image sensor as a line or a curve. This laser beam may be modelled as a point 
light source, which is constrained to pass to a narrow opening (Fig. 4). In POV-Ray a freeware 
raytracing software package (POV-Ray, 2003), the laser beam can be simulated with the code 
from Fig. 4 (b). Here, the laser rays start from origin, are projected in the positive direction of 
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the Z axis and the laser rays are going to be focused in YZ plane. The narrow opening has the 
dimensions L (large edge) and W (small edge) and is located at a distance D from the origin. 



Point light 




Narrow opening 
(a) Laser beam modelling principle 
Fig. 4. Laser beam modelling using POV-Ray 



"light_source { 
<0, 0, 0> 

color rgb <1, 0, 0> 
projectecLthrough { 
box { 

<-W/2, -L/2, D>, 
< W/2, L/2, D + eps> 
} 



} 



} 



(b) POV-Ray example 



The cameras used in the laser probe are be modeled as two standard perspective cameras, 
which may be implemented in POV-Ray by entering their parameters such as position, orien- 
tation and focal length. In the following text, only one of the two cameras will be described, 
as the other one is identical and symmetrical to the first one. For converting the image data 
into 3D coordinates, a pinhole camera model (Peng & Gupta, 2007) is used. 





Fig. 5. Laser sensor simulation: (a) planar laser beam, two cameras and a virtual workpiece; 
(b) Simulated images obtained from the two cameras using raytracing 



Let XYZ be the reference frame of the laser probe (Fig. 6(b) and 6(c)), and let xyz be the ref- 
erence frame of the CCD array from the camera (Fig. 6(a) and 6(b)). Referring to Fig. 6(b), 
the camera position and orientation with respect to the laser device is given by three scalar 
parameters: a, b and (p. 
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(a) CCD sensor and its reference frame 
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(b) Side view of the laser probe 



(c) Front view of the laser probe 



Fig. 6. Triangulation 



Using these notations, let P = (Px) Pyi Pz) me point of reflection of a laser ray, in the XVZ 
reference frame, and let p = (p x ; py) be the coordinate of the pixel at which the ray was 
detected on the CCD matrix, in xy reference frame. Knowing the 2D pixel coordinates p, the 
location of the 2D point P can be expressed using the triangulation equations (1): 



Px = 



f sin I (p — arctan -j- 



V* Pz 



Pv 
f tan I <p — arctan -j- 



+ b (1) 



where / 



H 



if the unit length is considered to be 1 pixel, i.e. the distance between two 



2 tan 7 

adjacent pixels on the CCD array. 

These equations are valid only under ideal conditions, i.e. when the camera and the laser 
sensor are perfectly aligned and there are no optical distortions from the camera lens. In 
practice, the transformation for converting the 2D pixel coordinates into 3D data expressed in 
milimetres is obtained using a calibration procedure. A look-up table model accounts for any 
nonlinear errors, especially lens distortion, and physical alignment between the camera and 
the laser is tuned using scalar offset parameters. 
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Fig. 7. Screenshot of the laser scanning system simulator 



2.2 Integration Issues 

A 3D point cloud model of the scanned part can be obtained by combining the measurements 
from the sensor with the instantaneous position of the robot. Aside from mechanical and 
electrical connections between the sensor and the laser probe, the two devices have to be 
synchronized. There are two operating modes supported by the sensor: 

• Stop and look 

• Buffered synchronization 

With the first method, the measurements from the sensor are read only when the robot is not 
moving, and has reached its programmed destination. The method is the easiest to implement, 
does not need any synchronization signals between the vision sensor and the robot, but it is 
also the slowest, being limited at around 1 or 2 sensor readings per second. It is used only for 
debugging purposes. 

The second method requires a trigger signal, which in the current implementation is sent from 
the vision sensor in the middle of the exposure period, from the sensor to the robot. When 
receiving the signal, the robot latches its instantaneous position, and stores it in a buffer. The 
trigger signal may be reversed, so the robot activates the vision sensor. The data from the robot 
and the sensor is collected on the PC and processed at a later time. This method allows sensor 
readings to be taken while the robot is still in motion, and close-spaced measurements can 
be taken at much higher rates, e.g. 50 readings/second. However, there may be a significant 
delay from the of data acquisition until the data is processed by the PC. In the system used 
here, the bottleneck is the Ethernet link between the robot and the PC, and the delay is usually 
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0.2 ... 0.3 seconds, and could reach 1 second. The buffers ensure that the data is matched 
properly even when high delays occur in communication. 

Since the system also has a 7 degree of freedom, the rotary table, its controller has to be 
also synchronized with the robot. If the table does not rotate while the sensor is actually 
scanning, there is no need for additional synchronization. If the table would have to rotate 
while scanning takes place, the rotary table controller would have to also listen to the trigger 
signal. 

Another issue in integrating the three components (sensor, robot and turntable) is the calibra- 
tion. For accurate 3D reconstruction, the system has to know, at every moment, the position 
of the table and the position of the sensor, both relative to robot base. Calibration issues are 
discussed in detail in (Borangiu et al., 2008b) and (Borangiu et al., 2009a). 

2.3 Surface Reconstruction from Point Cloud 

The raw output from the laser scanning system is a point cloud model, consisting of a huge and 
disorganised set of 3D points (X, Y and Z coordinates). This format is rarely used in practice; 
other models can be derived from it, such as the depth map or the polygon mesh. 
An example of 3D reconstruction is given in Fig. 8, when the scanned part was a small dec- 
orative object having 40 mm height. The scanning procedure was done in 16 passes, i.e. 8 
passes looking at the part from above and 8 passes from below. In each scan pass, the laser 
sensor was moved only in translation. Between two scan passes, the turntable was rotated in 
45 degree increments in order to get a complete 3D representation of the part surface. 
From each scan pass, the point cloud was transformed into a depth map using a straightfor- 
ward approach, mapping the farthest point to black and the closest point to white. From the 
depth map it was possible to obtain a mesh by taking 4 adjacent pixels and forming a quadri- 
lateral. The meshes from the 16 scans were stitched in MeshLab, an open source package for 
processing and editing large and unstructured 3D triangular meshes Cignoni (2008). 





(b) 

Fig. 8. 3D reconstruction example: (a) Photography of a decorative object, along with the laser 
stripe; (b) Point cloud from one scan pass; (c) Depth map computed from the point cloud; (d) 
Mesh model obtained from the depth map; (e) Complete 3D model, postprocessed in MeshLab 
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3. Automatic 3D Contour Following 




Fig. 9. 3D contour following using a sharp tool tip 
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Fig. 10. (a) Trajectory learning assisted by automatic edge recognition; (b) The two tool 
transformations: one for trajectory teaching, other for following it using the physical tool 
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A second application, described in (Borangiu et al., 2009b), uses the same profile sensor for 
teaching a complex 3D path which follows an edge of an workpiece, without the need to have 
a CAD model of the respective part. The 3D contour is identified by its 2D profile, and the 
robot is able to learn a sequence of points along the edge of the part. After teaching, the robot 
is able to follow the same path using a physical tool, in order to perform various technological 
operations, for example, edge deburring or sealant dispensing. For the experiment, a sharp 
tool was used, and the robot had to follow the contour as precisely as possible. Using the laser 
sensor, the robot was able to teach and follow the 3D path with a tracking error of less than 
0.1 milimetres. 

The method requires two tool transformations to be learned on the robot arm (Fig. 10(b)). The 
first one, Ti, sets the robot tool center point in the middle of the field of view of the laser 
sensor, and also aligns the coordinate systems between the sensor and the robot arm. 
Using this transform, any homogeneous 3D point P S ensor = (X, Y, Z, 1) detected by the laser 
sensor can be expressed in the robot reference frame (World) using: 

"zoorld = ^robot *L "sensor (2) 

where Ty f ot represents the position of the robot arm at the moment of data acquisition from 
the sensor. The robot position is computed using direct kinematics. 

The second transformation, Tj, moves the tool center point on the tip of the physical tool. 
These two transformations, combined, allow the system to learn a trajectory using the 3D 
vision sensor, having Tj_ active, and then following the same trajectory with the physical in- 
strument by switching the tool transformation to Tj. 
The learning procedure has two stages: 

• Learning the coarse, low resolution trajectory (manually or automatically) 

• Refining the accuracy by computing a fine, high resolution trajectory (automatically) 

The coarse learning step can be either interactive or automatic. In the interactive mode, the 
user positions the sensor by manually jogging the robot until the edge to be tracked arrives in 
the field of view of the sensor, as in Fig. 10(a). The edge is located automatically in the laser 
plane by a 2D vision component. In the automatic mode, the user only teaches the edge model, 
the starting point and the scanning direction, and the system will advance automatically the 
sensor in fixed increments, acquiring new points. For non-straight contours, the curvature is 
automatically detected by estimating the tangent (first derivative) at each point on the edge. 
The main advantage of the automatic mode is that it can run with very little user interaction, 
while the manual mode provides more flexibility and is advantageous when the task is more 
difficult and the user wants to have full control over the learning procedure. 
A related contour following method, which also uses a laser-based optical sensor, is described 
in (Pashkevich, 2009). Here, the sensor is mounted on the welding torch, ahead of the welding 
direction, and it is used in order to accurately track the position of the seam. 
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4. Conclusions 

This chapter presented two applications of 3D vision in industrial robotics. The first one al- 
lows 3D reconstruction of decorative objects using a laser-based profile scanner mounted on 
a 6-DOF industrial robot arm, while the scanned part is placed on a rotary table. The second 
application uses the same profile scanner for 3D robot guidance along a complex path, which 
is learned automatically using the laser sensor and then followed using a physical tool. While 
the laser sensor is an expensive device, it can obtain very good accuracies and is suitable for 
precise robot guidance. 
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1. Introduction 

In this chapter, a short description of the basic concepts about optical methods for the 
acquisition of three-dimensional shapes is first presented. Then two applications of the 
surface reconstruction are presented: the passive technique Shape from Silhouettes and the 
active technique Laser Triangolation. With both these techniques the sensors (telecameras 
and laser beam) were moved and oriented by means of a robot arm. In fact, for complex 
objects, it is important that the measuring device can move along arbitrary paths and make 
its measurements from suitable directions. This chapter shows how a standard industrial 
robot with a laser profile scanner can be used to achieve the desired d-o-f . 
Finally some experimental results of shape acquisition by means of the Laser Triangolation 
technique are reported. 

2. Methods for the acquisition of three-dimensional shapes 

In this paragraph the computational techniques are described to estimate the geometric 
property (the structure) of the three-dimensional world (3D), starting from ist 
bidimensional projections (2D): the images. The shape acquisition problem ( shape/model 
acquisition, image-based modeling, 3D photography) is introduced and all steps that are 
necessary to obtain true tridimensional models of the objects, are synthetized [1], 
Many methods for the automatic acquisition of the shape object exist. One possible 
classification of the methods for shape acquisition is illustrated in figure 1. 
In this chapter optical methods will be analyze. The principal advantages of this kind of 
techniques are the absence of contact, the rapidity and the economization. The limitations 
include the possibility of being able to acquire only the visible part of the surfaces and the 
sensibility to the property of the surfaces like transparency, brilliance and color. 
The problem of image-based modeling or 3D photography, can be described in this way: the 
objects irradiate visible light; the camera capture this "light", whose characteristics depend 
on the lighting system of the scene, surface geometry, reflecting surface; the computer 
elaborates the light by means of opportune algorithms to reconstruct the 3D structure of 
the objects. 
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Fig. 1. Classification of the methods for shape acquisition [1] 

In the figure 2 is shown an equipment for the shape acquisition by means two images. 




Fig. 2. Stereo acquisition 

The fundamental distinction between the optical techniques for shape acquisition, regards 
the use of special lighting sources. In particular, it is possible to distinguish two kinds of 
optical methods: active methods, that modify the images of scene by means of opportune 
luminous pattern, laser lights, infrared radiations, etc., and passive methods, that analyze 
the images of the scene without to modify it. The active methods have the advantage to 
concur high resolutions, but they are more expensive and not always applicable. The 
passive methods are economic, they have fewer constraints obligatory, but they are 
characterized by lower resolutions. 

Many of the optical methods for the shape acquisition have like result an image range, that 
is an image in which every pixel contains the distance from the sensor, of a visible point of 
the scene, instead of its brightness (figure 3). An image range is constituted by measures 
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(discrete) of a 3D surface respect to a 2D plan (usual the plane image sensor) and therefore 
it is also called: 2.5D image. The surface can be always expressed in the form Z = f(X, Y), if 
the reference plane is XY. A sensor range is a device that produces an image range. 




Fig. 3. Brightness reconstruction of an image [1] 



Below optical sensor range is any optical system of shape acquisition, active or passive, that 
is composed of equipment and softwares and that gives back an image range of the scene. 
The main characteristics of a sensor range are: 

• resolution: the smallest change of depth that the sensor can find; 

• accuracy: diffrence between measured value (average of repeated measures) and 
true value (it measures the systematic error); 

• precision: statistic variation (standard deviation) of repeated measures of a same 
quantity (dispersion of the measures around the average); 

• velocity: number of measures in a second. 

2.2 From the measure to the 3D model 

The recovery of 3D information, however, does not exhaust the process of shape acquisition, 

even if it is the fundamental step. In order to obtain a complete model of an object, or of a 

scene, many images range are necessary, and they che they must be aligned and merged 

with each other to obtain a 3D surface (like poligonal mesh). 

The reconstruction of the model of the object starting from images range, previews three 

steps: 

• adjustment: (or alignment) in order to transform the measures supplied from the 
several images range in a one common reference system; 

• geometric fusion: in order to obtain a single 3D surface (typically a poligonal 
mesh) starting various image range; 

• mesh simplification: the points given back by a sensor range are too many to have 
a manageable model and the mesh must be simplified. 

Below the first phase will be described above all, the second will be summarily and the third 
will be omitted. 

An image range Z(X,Y) defines a set of 3D points (X,Y,Z(X,Y)), figure 4a. In order to obtain 
a surface in the 3D space (surface range) it is sufficient connect between their nearest points 
with triangular surfaces (figure 4b). 
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Fig. 4. Image range result (a) and its surface range (b) [1]. 

In many cases depth discontinuities can not be covered with triangles in order to avoid 
making assumptions that are unjustified on the shape of the surface. For this reason it is 
desirable to eliminate triangles with sides too long and those with excessively acute angles. 

2.3 Adjustment 

The sensors range don't capture the shape of an object with a single image, many images are 

needed, each of which captures a part of the object surface. The portions of the surface of the 

object are obtained by different images range, and each of them is made in its own reference 

system ( that depends on sensor position). 

The aim of adjustment is to expres all images in the same reference system, by means of an 

opportune rigid transformation (rotation and translation). 

If the position and orientation of the sensor are known, the problem is resolved banally. 

However in many cases, the sensor position in the space is unknown and the 

transformations can be calculated using only images data, by means of opportune 

algorithms, one of these is ICP (Iterated Closest Point). 

In the figure 5, on the left, eight images range of an object are shown, each in its own 

reference system; on the right, all images are were superimposed with adjustment 

operation. 




Fig. 5. Images range and result of adjustment operation [1] 
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2.4 Geometric fusion 

After all images range data are adjusted in one reference system, they be united in a single 
shape, represented, as an example, by triangular mesh. This problem of surface 
reconstruction, can be formulated like an estimation of the bidimensional variety which 
approximates the surface of the unknown object by starting to a set of 3D points. The 
methods of geometric fusion can be divided in two categories: 

• Integration of meshes: the triangular meshes of the single surfaces range, are 
joined. 

• Volumetric fusion: all data are joined in a volumetric representation, from which a 
triangular mesh is extracted. 

2.4.1 Integration of meshes 

The techniques of integration of meshes aim to merge several 3D overlapped triangular 
meshes into a single triangular mesh (using the representation in terms of surface range). 
The method of Turk and Levoy (1994) merges overlapped triangular meshes by means of a 
technique named "zippering". The overlapping meshes are eroded to eliminate the overlap 
and then it is possible to use a 2D triangolation to sew up the edges. To make this the 
points of the two 3D surface close to edges, must be projected onto a plane 2D. 
In the figure 6, on the left, two aligned surface are shown, and on the right, the zippering 
result is shown. 




Fig. 6. Aligned surface and zippering result 



The techniques of integration of meshes allow the fusion of several images range without 
losing accuracy, since the vertices of the final mesh coincide with the points of the measured 
data. But, for the same reason, the results of these techniques are sensitive to erroneous 
measurements, that may cause problems in the surface reconstruction. 

2.4.2 Volumetric Fusion 

The volumetric fusion of surface measurements constructs an intermediate implicit surface 
that combines the measurements overlaid in a single representation. The implicit 
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representation of the surface is an iso-surface of a scalar field f(x,y,z). As an example, if the 

function of field is defined as the distance of the nearest point on the surface of the object, 

then the implicit surface is represented by f(x,y,z) = 0. This representation allows modeling 

of the shape of unknown objects with arbitrary topology and geometry. 

To switch from implicit representation of the surface to a triangular mesh, it is possible to 

use the algorithm Marching Cubes, developed by Lorensen e Cline (1987) for the 

triangulation of iso-surfaces from the discrete representation of a scalar field (as the 3D 

images in the medical field). The same algorithm is useful for obtaining a triangulated 

surface from volumetric reconstructions of the scene (shape from silhouette and photo 

consistency). 

The method of Hoppe and others (1992) neglects the structure of the data (surface range) 

and calculates a surface from the unstructured "cloud" of points. 

Curless and Levoy (1996) instead, take advantage of the information contained in the images 

range in order to assign the voxel that lie along the sight line that, starting from a point of 

the surface range, arrives to the sensor. 

An obvious limitation of all geometric fusion algorithms based on an intermediate structure 

of discrete volumetric data is a reduction of accuracy, resulting in the loss of details of the 

surface. Moreover the space required for the volumetric representation grows quickly when 

resolution grows. 

2.5 Optical methods for the shapes acquisition 

All computational techniques use some indications in order to calculate the shape of the 
objects starting from the images. Below the main methods divided between active and 
passive, are listed. 

Passive optical methods: 

• depth from focus/ defocus 

• shape from texture 

• shape from shading 

• stereo-photometric 

• stereopsis 

• shape from silhouette 

• shape from photo-consistency 

• structure from motion 

Active optical methods: 

• active defocus 

• active stereo 

• active triangolation 

• interferometry 

• flight time 

All the active methods, except the last one, employ one or two cameras and a source of 
special light, and fall in the wider class of the methods with structured lighting system. 
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3. Three-dimensional reconstruction with technique: Shape from Silhouettes 

In this paragraph one of the passive optical techniques of 3D reconstruction will be 
introduced in detail, with some obtained results. 

3.1 Principle of volumetric reconstruction from shapes 

The aim of volumetric reconstruction is to create a representation that describes not only the 
surface of a region, but also the space that it encloses. The hypothesis is that there is a 
known and limited volume, in which the objects of interest lie. A 3D box is modelled to be 
an initial volume model that contains the object. This box is divided in discrete elements 
called voxels, that are three-dimensional equivalent of bidimensional pixel. The 
reconstruction coincides with the assignment of a label of occupation (or color) to each 
element of volume. The label of occupation is usually binary (transparent or opaque). 
Volumetric reconstruction offers some advantages compared to traditional stereopsi 
techniques: it avoids the difficult problem of finding correspondences, it allows the explicit 
handling of occlusions and it allows to obtain directly a three-dimensional model of the 
object (it is not necessary to align parts of the model) integrating simultaneously all sights 
(which are the order of magnitude of ten). 
Like in the stereopsi, the cameras are calibrated. 

Below one of the many algorithms developed for the volumetric reconstruction from 
silhouettes of an object of interest is described: shape from silhouettes . 

Shape From Silhouettes is well-known technique for estimating 3D shape from its multiple 
2D images. 

Intuitively the silhouette is the profile of an object, comprehensive of its inside part. In the 
"Shape from Silhouette" technique silhouette is defined like a binary image, which value in 
a certain point (x, y) underlines if the optical ray that passes for the pixel (x, y) intersects or 
not the object surface in the scene. In this way, Every point of the silhouette, respectively of 
value "1" or "0", identifies an optical ray that intersects or not the object. 
To store the labels of the voxel it is possible to use the octree data structure. The octree are 
trees with eight ways in which each node represents a part of space and the children nodes 
represents the eight divisions of that part of space (octants). 

3.1.1 Szeliski algorithm 

The Szeliski algorithm allows to build the volumetric model by means of octree structure. 
The octree structure is constructed subdividing each cube in eight part (octants), starting 
from the root node that represents the initial volume. Each cube has associated a color: 

- black: it represents a occupied volume; 

- white: it represents an empty volume; 

- gray: it represents an inner node whose classification is still uncertain. 

For each octant, it is necessary to verify if its projection in image i, is entire contained in the 
black region. If that happens for all N shapes, the octant is labeled as black. If instead, the 
octant projection is entire contained in the background (white), also for a single camera, the 
octant is labeled as white. If one of these two cases happens, the octant becomes a leaf of the 
octree and it is not more tried, otherwise, it is labeled as gray and it is subdivided in eight 
parts. In order to limit the dimension of the tree, gray octants with minimal dimension, are 
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labeled as black. At the end of process it is possible to obtain an octree that represents 3D 

object structure. 

An example of volumetric model construction with Szeliski algorithm is shown in figure 7. 





Fig. 7. Model reconstruction with Szeliski algorithm 

3.1.2 Proposed method 

The analysis method is to define a voxel box that contains the object in three dimensional 
space and to discard subsequently those points of the initial volume that have an empty 
intersection with at least one of the cones of the shapes obtained from the acquired images. 
The voxel will have only binary values, black or white. 

The algorithm is performed by projecting the center of each voxel into each image plane, by 
means of the known intrinsic and extrinsic camera parameters. If the projected point is not 
contained in the silhouette region, the voxel is removed from the object volume model. 

3.2 Images elaboration 

Starting from an RGB image (figure 8), it is analized and, by means of segmentation 
procedure, it is possible to obtain object silhouette reconstruction. 




Fig. 8. RGB image 



Segmentation is the process by means of which the image is subdivided in characteristics of 
interest. This operation is based on strong intensity discontinuities or regions that introduce 
homogenous intensity on the base of established criteria. There are four kinds of 
discontinuities: points, lines, edge, or, in a generalized manner, interest points. 
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In order to separate an object from the image background, the method of the threshold s is 
used: each point (u,v) with f(u,v) > s (f(u,v) < s) is identified like object, otherwise like 
background. The described elaboration is used to identify and characterize the various 
regions that are present in each image; in particular, by means of this technique it is possible 
to separate the objects from the background. 

The first step is to transform RGB image in gray scale image (figure 9), in thi way the 
intensity value becomes a threshold to identify the object in the image. 




Fig. 9. Gray scale image 

The gray scale image is a matrix, whose dimensions correspond to number of pixel along the 
two directions of the sensor, and whose elements have values in range [0,255]. 
Subsequently, a limited set of pixel that contains the projection of the object in the image 
plane, is selected, so it is possible to facilitate the segmentation procedure (figure 10). 




Fig. 10. Object identification 



In gray scale image, pixel with intensity f(u,v) different from that of points that are not 
representative of object (background T), are chosen. This tecnique is very effective, if in the 
image there is a real difference between object and background. 

For each pixel (u, v) unit value or zero, respectively, is assigned, if it is an optical beam 
passing through the object or not (figure 11). 
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Fig. 11. The computed object silhouette region 



3.3 Camera calibration 

It is necessary to identify all model parameters in order to obtain a good 3-D reconstruction. 

Calibration is a basic procedure for the data analysis. 

There are many kind of procedure to calibrate a camera system, but in this paragraph the 

study of the calibration procedures will not be discussed in detail. 

The aim of calibration procedure is to obtain all intrinsic and estrinsic parameters of the 

camera system. Calibration procedure is based on a set of images, taken with a target placed 

in different positions that are known in a base reference system. A least square optimization 

allows to identify all parameters. 

3.4 The proposed algorithm 

The result of image elaboration is the object silhouette region for each image with pixel 
coordinates and centroid coordinates of these region in image reference system, (Fig. 11). 
By means of calibration parameters (intrinsic and estrinsic), it is possible to evaluate, for 
each image, an homogeneous transformation matrix, between the image reference system of 
each image and a base reference system. 

The first step is to discretize a portion of work space by mean an opportune box divided in 
voxels (figure 12). In this operation, it is necessary to choose the number of voxels, the 
dimension of box and its position in a base reference system. The position of voxel is chosen 
evaluating the intersections in base reference system, of the camera optical axis that pass 
through silhouette centroids of at least, two images. Subsequently it is possible to divide the 
initial volume model in a number of voxels according to the established precision, and it is 
possible to evaluate the centers of voxels in base reference. 




Fig. 12. Voxel discretization of workspace. 
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For each of the silhouettes, the projection of the centre of the voxels in the image plane can 
be obtained as follows: 



{*} j =[M] i -{w} 



i = l P 



i q 



(i) 



Where p is the number of silhouettes, q is the number of the voxels and [M]j is the 
transformation matrix between the base frame and the image frame. 

The algorithm is performed by projecting the center of each voxel into each image plane, by 
means of the known intrinsic and extrinsic camera parameters. If the projected point is not 
contained in the silhouette region, the voxel is removed from the object volume model. This 
yields the following relation: 



= [+]j=[M] p -{w} 



j=l,...,q l<u<n l<v<m n) 



Where n and m are the number of pixels along the directions u and v, respectively. So a 
matrix [A] having dimension [n,m] is written; the values of the elements of this matrix is one 
if corresponds to a couple of coordinates (u,v) obtained by eq. (2) that belongs to the object 
silhouette in the image; otherwise the value is zero: 



[A]:=a 



hk 



h = Uj,k = V; 



(3) 



A set of pixels having coordinates (u,v), belonging to the first silhouette, is defined as 
follows: 

Qj := (u,v)! (4) 

The points of the image that belong to the silhouette are defined as follows: 
[1 (u, v)cQ 1 



Il(U,V) = 



(u,v)<zCl 1 



1 < u <n 1 < v < m 



(5) 



By the product among the matrix defined in eq. (3) with the matrix in eq. (5), the following 
set of indexes is obtained: 



j:[A].[I 1 ] = l 



l<u<n 



1< v<m 



(6) 



Now the points (in the base frame) which indexes are integer numbers belonging to the set 
j are considered. These points are used as starting points to repeat the same operations, 
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described by eq. (2), for all the other images. This procedure is recursive and is called space 
carving technique. 




Fig. 13. Scheme of space carving technique. 



3.5 Evaluation of the resoluction 

The choosen number of voxels defines the resoluction of the reconstructed object. 

Consider a volume which dimensions are l x , l y and l z and a discretization along the three 

directions: A x , A y e A z , the object resolutions that can be obtained in the three directions are: 



_ !x . _ l v . _ l z 

a x — , a v — , a z — 

A x A y A 2 



(7) 



In figure 14 two examples of reconstruction with different resolution choices are shown. 




Fig. 14. Examples of reconstructions with different resolutions. 



It must be pointed out that the choice of the initial box (resolution) depends on the optical 
sensor adopted. It is clear that an object near to the sensor will appear bigger than a far one, 
hence the error achieved for the same unit of discretization is increased with the distance for 
a given focal length. 
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w. w. 

a °u = 5 u'~r; a<: v =5 v~r (8) 

Obviously it must be checked that the choosen initial resolution is not bigger than the 
resolution that the optical sensor can achieve. To this pourpose, once the object has been 
reconstructed, the minimum dimension that can be recorded by the camera is computed by 
eq. (8); the latter is compared with the resolution initially stated. 

This techique is rater slow because it is necessary to project each voxel of the initial box in 
the image plane of each of the photos. Moreover, a big number of phots is required and the 
procedure can not be made in real -time. It must also be noted, however, that this procedure 
can be used in a rater simple way in order to obtain a rough evaluation of the volume and of 
the shape of an object. 

This techinque is very suitable to be assisted by a robot arm. Infact the accuracy of the 
reconstruction obtained depends on the number of images used, on the positions of each 
viewpoint considered, on the camera's calibration quality and on the complexity of the 
object shape. By positioning the camera on the robot, it is possible to know, exactly, not only 
the characteristics of the camera, but also the position of the camera reference frame in the 
robot work space. Therefore the camera intrinsic and extrinsic parameters are known 
without a vision system calibration and it's easy to make an elevated number of photos. That 
is to say, it could be possible to obtain vision system calibration, robot arm mechanical 
calibration and trajectories recording and planning. 

4. Three-dimensional reconstruction by means of Laser Triangolation 

The laser triangulation technique maily permits higher operating speed with a satisfacting 
quality of reconstruction. 

4.1 Principle of laser triangolation 

In this paragraph is described a method for surface reconstruction, that uses of a linear laser 
emitter and a webcam, and uses triangulation principle applied to a scanning belt on object 
surface. 

Camera observes the intersection between laser and object: laser line points in image frame, 
are the intersections between image plane and optical rays that pass through the intersection 
points between laser and object. By means of a transformation matrix, it is possible to 
express the image frame coordinates, in pixel, in a local reference frame. In figure 15 a) is 
shown a scheme of scanning system: {W} is local reference frame, {1} is image frame with 
coordinates system {u,v}, and {L} is laser frame. {L2} is laser plane that contains laser knife 
and scanning belt on the surface of object, and it coincides with (x,y) plane of laser frame {L}. 
Starting from the coordinates in pixel (u,v), in image frame, it is possible to write the 
coordinates of the scanning belt on the object surface, in camera frame by means of equation 
(9). Camera frame is located in camera focal point figure 15 b). 
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with: 

(uo,vo): image frame coordinates of focal point projection in image plane; 
(5x, 5 y ): physical dimension of sensor pixel along direction u and v; 
f: focal length. 




Fig. 15. Scheme of scanning system 

It is possible to write the expression of the optical beam of a generic point in the image 
frame, that can be identified by means of parameter t. 



x c =(u-u )5 u t 

y c = ( v - v o) s v t 

z r =ft 



(10) 



Laser frame {L} is rotated and translated respect to camera frame, the steps and their 

sequence are: 

Translation Axi c along axis x c ; 

Translation Ayj c along axis y„ 

Translation Azi c along axis z c ; 

Rotation cpi c around axis z c ; 

Rotation 6i c around axis y c ; 

Rotation \|/i c around axis x c ; 

Hence in equation (11) the transformation matrix between laser frame and camera frame is: 
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Laser plane {L2} coincides with (x,y) plane of laser frame {L}, so it contains three points: 



{p}, ={0,0,0,l} T ;{q}! ={l,0,0,l} T ;{r} 1 ={0,0,1,1} 1 



(12) 



In camera frame: 



iPx'PyPz' 1 )? =r T lf fekilx'^ylz' 1 }!: 



(13) 



It is possible to obtain laser plane equation in camera frame, solving equation (14) in xc, yc 
and zc. 



det 



X c-Px Yc-Py 

q x -p x q y -p> 



Pz 
Pz 



(14) 



If it is: 



M v = det 



M, = det 



q y -p y q z -p z 

r y-Py r z-Pz 

q x -Px q y -p y 

r x-Px r y-Py 



M y = det 



q x -Px q z -p z 

r x-Px r z-p z 



equation in camera frame, is: 

(^ -Px)M x -(y c -p y )M y +(z c - Pz )M z = 



(15) 



It is possible to evaluate coordinates xc, yc e zc, in camera frame, solving system (16) with 
unknown t: 
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x c =(u-u )8 x t 

Yc =( v - v o)8yt 

z c =ft 

(x c - Px )M x -(y c - Py )M y +(z c - Pz )M z = 



(16) 



The solution is: 



p x M x -p M +p z M z 



(u-u )5 x M x -(v-v )5 y M y +fM 



(17) 



Equation (17) permits to compute in the camera frame, the points coordinates of the 
scanning belt on the object surfaces, starting to its image coordinates (u,v). In this way it is 
possible to carry out a 3-D objects reconstruction by means of a laser knife. 

4.2 Detection of the laser path 

A very important step for 3-D reconstruction is image elaboration for the laser path on the 
target, [5, 6, 7] , the latter is shown in figure 16. 




Fig. 16. Laser path image. 

Image elaboration procedure permits to user to choose some image points of laser line, in 
order to identify three principal colours (Red, Green, and Blue) of laser line, figure 17. 
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Fig. 17. Image matrix representation. 

With mean values of scanning belt principal colours, it is possible to define a brightness 
coefficient of the laser line, according to relation (18): 
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max(mean(R),mean(G),mean(B)) + min(mean(R),mean(G),mean(B)) 



By means of relation (19), an intensity analysis is carried out on RGB image. 

, max(R,G,B) + min(R,G,B) 
L(u,v) = 



(19) 



By equation (19), the matrix that contains the three layers of RGB image, is transformed in a 
matrix L, that represents image intensity. This matrix represents same initial image, but it 
gives information only about luminous intensity in each image pixel, and so it is a grayscale 
expression of initial RGB image, figure 18 a) and b). 




a) b) c) 

Fig. 18. a)RGB initial image ; b)grayscale initial image L ; c) matrix lb. 

With relation (20), it is possible to define a logical matrix lb. Matrix lb indicates pixels of 
matrix L with a brightness in a range of 15% brightness coefficient s. 



Ib(u,v)= 



1 se 0.85 s<L(u,v)< 1.15 s 
otherwise 



(20) 



In figure 18 c), matrix lb is shown. 




00000000 00 00 00 
11 0001 10 00 01 00 

oTT i i TTo" i i'T i i "o" o 

1111111 11 1 11 10 

~T i 1TTT1 i'T i i TT 

100000000010111 
1001 100000101 1 1 

00001 1 1 1 0001000 
000000000000000 



Fig. 19. lb representation. 

In matrix lb, the scanning belt on the object surface (see fig. 19) is represented by means of 
the pixel value one. The area of the laser path in the image plane depends on the real 
dimension of laser beam and on external factors such as: reflection phenomena, inclination 
of object surfaces. 

3-D reconstruction procedure is based on triangulation principle and it doesn't consider the 
laser beam thickness, so it is necessary to associate a line to the image of scanning belt. 
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Since the laser path in the image plane is rather horizontal, a geometrical mean is 
computed on, on the columns of the matrix lb , that is to say: in the opposite direction to 
wider extension of the laser path in the image. This is shown in equation (21). 



Q, := {u,v};Q := {u,v} => 

£ul b (u,v) 

{u = — — ■£ co, v = v : £I b (u,v) *■ 0} 

£I b (u,v) U/Ve n 

u.veQ 



(21) 



Then a matrix hp is also defined as follows: 



h b (u,v) = 



{u,v} e Q. 



otherwise 



(22) 



Matrix hb is hence a logical matrix with same dimension of lb in which that represents laser 
image like a line, figure 20. This line is centre line of scanning belt on object surface in 
image. 



































M 























> 




-1 













1 











y o 


\ 





1_ 


J_ 


1 


yo 







1 


1 1 


y 


0' 











0' 


rJ -11 































































































































Fig. 20. hb representation. 

The set of transformations (19), (20) and (21) represents the image elaboration, that is 
necessary to identify a laser line in image. The points of this line are used in 3-D 
reconstruction procedure. 



4.3 Calibration procedure 

It is necessary to identify all model parameters in order to obtain a good 3-D reconstruction. 
Calibration is a basic procedure for the data analysis, [4, 6, 8]. For this reason, it was 
developed a calibration procedure for a classic laser scanner module. 

The laser scanner module is composed by a web-cam with resolution 640x480 pixel and a 
linear laser. The calibration test rig is realized with a guide on which is fixed the laser 
scanner module and a digital micrometer with a target, figure 21 a). 
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a) b) 

Fig. 21. a) Laser scanner module calibration test rig; b) fixed frame OfXfyfZf . 

A fixed frame OfXfyfZf has origin in a vertex of the rectangular target with dimension a=68 

mm and b=74.5 mm, like it is shown in figure 21 b). Target movements are indicated with 

Azbf. 

In figure 22 some steps of calibration procedure are shown. 




Fig. 22. Calibration procedure steps. 



Calibration procedure is based on a set of images, taken with the target placed in different 
positions respect to the laser scanner module. A least square optimization allows to identify 
all parameters. In figure 23 are shown 10 images used for calibration, of scanning belt, with 
10 different Azbf. 




Fig. 23. Images used for calibration. 

The aim of calibration procedure is to obtain the parameters that define the relation between 
laser frame LxLyLzL and camera frame O c x c ycZc, figure 15. 

In every calibration step, it is possible to define transformation matrix between fixed frame 
OfXfyfZf and target frame: 



[ £ T b |=[l 0;0 1 0;0 1 -Az fb ;0 1] 



(23) 
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The transformation matrix between fixed frame OfXfyfZf and camera frame O c x c y c z c can be 
optained analogous to matrix [ c Ti], and it is a function of six parameters: 

[ f T c ]= f(Ax cf ,Ay cf , Az cf , ¥cf ,e cf ,cp cf ) = i(n cf ) (24) 

with ncf set of parameter of transformation [ f T c ]. 
Equation (11) can be written as: 

[ C T,] = g(Ax lc ,Ay lc ,Azi c/ y lc ,9 lc/ <p lc ) = f(7i k ) (25) 

with nlc set of parameter of transformation [ C TJ. 

For each step of the calibration procedure it is possible to evaluate the coordinates of the 

points of the laser line in the camera frame, according to equations (13) and (21): 

{x c/ y c/ z c/ l}? =f(n lc ,f).{u,v / / lh T (26) 

By means of equation (22), it is possible to write: 



[XoYitZfA}! =[ lT c\ {XoYoZo 1 }! = 

[ f T c }' 1 .f(jq c/ f).{u / vAl}i T =a>(ji rf/ ii Ic/ f)-{u / v / / lh T 



(27) 



The optimization problem can be defined as: 



minF(p) 2 (28) 

peR 13 

F(p) is a vectorial function defined as: 

F(p) = {(zf 1 -zj)-(Az^ -AzJ,l 1 ),min(xj), 

max(xf)-a,max(z"f)-min(zf), (29) 

max(yf)-min(yf),min(yf ),min(zf )} 

With a least square optimization, the unknown parameters of set [ncf, nlc, f] are identified. 

An interactive GUI was developed to allow the user to acquire images, to execute 

optimization and to verify the results. 

In figure 24, a graphical result of calibration is shown: it is possible to observe the camera 

frame position, the laser beam (represented by the quadrilateral on the left), and the 3-D 

reconstructions of scanning beam on target surface, for each image used in the calibration 

procedure. 
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Fig. 24. Calibration results 

4.4 System accuracy 
4.4.1 Camera error 

An obvious source of camera calibration error arises from the spatial quantization of the 
image. However, many times the image event of interest spans many pixels. It is then 
possible to calculate the centroid of this event to sub-pixel accuracy, thereby reducing the 
spatial quantization error. Limiting the sub-pixel accuracy is the fact that in general the 
perspective projection of an object's centroid does not equal the centroid of the object's 
perspective projection. However, often times the error incurred from assuming the centroid 
of the projection is the projection of the centroid is quite small [14]. 
Camera accuracy is a function of its distance from observed object. 

If an unitary variation of pixels in the directions u and v is considered, from the equation 
(10) is gotten: 

x c = (u + l-u )5 x -^ 

y c =(v + l-v )8 y ^ (30) 

Calculating the difference respectively between x c and y c after and before the variation is 
gotten the accuracy of the system, that is the variation of x c and y c that it can be gotten for a 
minimum variation in terms of pixels in the image frame: 

Ax c = x c (u + l)-x c (u) 

A yc=yc(v + i)-y c (v) 

The image accuracies in directions x c and y c are: 

a c -S Zc 
a * -°u ~T 
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y -«v 



(31) 



4.4.2 Scanner module error 

It is possible to define an accuracy expression for 3D-reconstruction that can be obtained by 

means of laser scanner module, according to equations (16) and (17). 

A variation (a, P) of image coordinates (u,v), generates a variation of parameter t of equation 

(17): 



At: 



(PxM x -p y M + Pz M z )-(a8 x M x -pS M ) 



y V 



(uS x M x + a8 x M x -v8 y M -p8 y M y + fM z )-(uS x M x -v8 y M y +fM z ) 



(32) 



where: 

- a pixel variation in u direction; 

- P pixel variation in v direction; 

The variation of parameter t, allows to define an expression of accuracy of 3D 
reconstruction. In fact, by means of equation (16), it is possible to obtain the variation of 
coordinates in camera frame in function of variation of image coordinates: 



Ax r = oS,- 



Ay c = PS, 



p x M x -p M +p z M 



Az r = Atf 



(u - u + a)S x M x - (v - v + P)5 y M y + fM 

p x M x -p y M y +p z M z 

(u - u + a)5 x M x - (v - v + p)S y M y + fM, 



-+(u-u )S x -At 
- + (v-v )S y -At 



(33) 



An unitary variation of image coordinates (a=l and P=0, or a=0 and P=l, or a=l and P=l), 
allows to define three accuracy parameters of scanner laser 3D reconstruction: 



a SL x = Ax c 
a SL y = Ay c 
a SL z = Az r 



with 



(a,P) = (0,l) 

or 
(a,P) = (l,0) 

or 
(a,P) = (l,l) 



(34) 
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Fig. 25. Error of the accuracy 

As it has shown in the figure 25, the worst accuracy of the laser scanner for a pixel variation 
is in the direction z c . Besides it can be seen that az has minimum value for values of v = dv 
and it grows up to v = 1 with a non-linear law. 



4.4.3 Laser precision 

Most of the outlier and other erroneous points are caused by reflections. In these cases, the 

high energy laser beam is reflected from mirroring surfaces such as metal or glass. 

Therefore, too much light hits the sensor of the camera and so-called blooming effects occur. 

In other cases, a direct reflection may miss the camera. In addition, a part of the object may 

lie in the path from the projected laser line to the camera causing a shadowing effect. All 

these effects are responsible for gaps and holes. At sharp edges of some objects, partial 

reflections appear. In addition, craggy surfaces cause multiple reflections and, therefore, 

indefinite point correlations. 

Furthermore, aliasing effects in the 2D image processing of laser beam, lead to high 

frequent noise in the generated 3D data [15]. 

The laser beam thickness in the image, can vary because of the above described effects, but 

3-D reconstruction procedure is based on triangulation principle and it doesn't consider this 

phenomenon. In fact, the detection of laser path allows to identify a line in image with a 

thickness of a pixel. 

The real laser beam thickness and its path thickness in the image, must be considered to 

evaluate the precision of 3D reconstruction. 

The accuracy a c z becomes worse, if a thickness parameter is considered. This parameter is a 

generalized measure of laser beam thickness in pixel, and it can be expressed with two 
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components: thu thickness measure along direction u in image frame and thv thickness 
measure along direction v in image frame. 

An expression of 3D reconstruction accuracy in direction z of camera frame, can be obtained 
by means of equation (11), in which parameters (a, P) are the generalized laser beam 
thickness (thu, thv): 



a x 


= Ax 


a y 


= Ay 


a-, 


= Az 



• with =s> (a,(3) = (th u /2,th v /2) 



(35) 



The equations (32) define the resolution of the laser scanner 3-D reconstruction, and they 
allows to evaluate the accuracy of each point coordinates that is obtained with laser beam 
image elaboration. 

4.5 Scanner range 

Another characteristic of a 3D laser scanner is the minimum and the maximum distance 
between a generic point of a surface and the image plane. These parameters define the range 
of the scanning procedure. Decreasing the angle of inclination of the laser plane respect to 
the plane x c z c of the camera frame at a respective fixed distance s the range of scanning 
increases (figure 26). 




Fig. 26. The minimum and the maximum distance between a generic point of a surface and 
the image plane 



This is not a good solution since to decrease of this angle the accuracy worsens notably as is 
shown in figure 27. 
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Fig. 27. Accuracy diagram. 



For the considered system with values s = 90 mm and 
525 mm and min(zc) = 124 mm; 



23° it have been gotten: max(zc) 



5. Experimental results 

To evaluate the accuracy of the laser scanner system, the latter was fixed on a robot arm; in 
this way it was possible to capture a lot of shape information of the object from different 



5.1 The test rig 

5.1.1 Laser Scanner Module 

Our rig, is based on a laser profile, that essentially consists in a line laser and a camera. The 
laser beam defines a "laser plane" and the part of the laser plane that lies in the image view 
of the camera is denoted the "scanning window", figure 28. 




Fig. 28. Scanner module 



The laser scanner device was realized by assembling a commercial linear laser and a 
common web-cam. 
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5.1.2 The Robot 

In order to optimize the accuracy of the reconstruction resulting model, scanning should be 
adapted to the shape of the object. One way to do that is to use an industrial robot to move a 
laser profile scanner along curved paths. 

The scanner laser module was mounted on a revolute robot with three d.o.f., designed and 
assembled in our Department, figure 29. 




Fig. 29. Revolute robot 



The robot serves as a measuring device to determine the scanning window position and 
orientation in 3D for each camera picture, with a great precision. All scan profiles captured 
during a scan sequence must be mapped to a common 3D coordinate system, and to do this, 
positional information from the robot were used [11]. Figure 30 shows the equipment at 
work. 




Fig. 30. The robot scanning system 



The authors have developed a solution where the robot controller and scanner software 
work separately during a scan sequence that will be described in the following paragraphs. 
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5.2 The laser scanner on the robot model 

When the laser scanner module is installed on the robot, figure 30, it is possible to use 
positional information from robot to determine the scanning window position and 
orientation in 3D. 

Defining [DH] as the transformation matrix between coordinates in the robot base frame 
(the fixed one) and those in frame 3 (the one of the last link), figure 31, for the coordinates of 
a generic point P exists this relationship: 



{P} =[DH].{P} 3 



(36) 



The matrix [DH] depends on 9 constant kinematic structure parameters, that are known, 
and 3 variable joints position parameters that are measurable by means of robot control 
system. 




12 









Fig. 31. Revolute robot scheme 

Knowing the transformation matrix [ C T3] between the camera frame and the frame of the 
robot last link, it's possible to obtain a transformation matrix between the camera frame and 
the frame 0, figure 32. 

{P} c =[ c T 3 HDHrMP}o (37) 




Fig. 32. Camera reference system. 



542 



Advances in Robot Manipulators 



By means of the equations (16), (17) and (29), the relationship between image coordinates 

(u,v) of the laser path and its coordinates in the robot base frame 0, is defined. By means of 

these equations, it is possible to reconstruct the 3D points in the robot base frame, of the 

intersection between the laser line and the object. 

Robot positioning errors do not influence the 3D reconstruction, because each image is 

acquired and elaborated in a real robot position, that is known by means of robot encoders 

[12]. 

5.3 The data capture and the registration 

The scanner video camera captures profiles from the surface of the object as 2D coordinates 
in the laser plane. During a scan sequence the laser scanner module is moved in order to 
capture object images from different sides and with different angles-shot according to the 
shape of the object. 

In Matlab, an interactive GUI was developed in order to allows users to acquire and to 
elaborate data, figure 33. For each camera picture, along the scan path, the scanner derives a 
scan profile built up of point coordinates, in real-time. 

The first step present in the GUI is the load of the calibration parameters that are composed 
by the laser scanner parameters and the matrix [ C T3] . The second step is to filter the pixels of 
the laser path from the image, to do this, there are some regulations: the identification of the 
intensity of selected pixels, the calculus of the threshold and other regulations of the camera 
settings. The third step is to write the 3 joint position parameters of the robot in the window 
"position" , after this, clicking on the button "Image" the software save all the information, 
necessary for the reconstruction, in the workspace of the Matlab. Clicking on the button "3D 
generation" the software calculates the 3D positions of the laser path in the robot base 
frame, and the result is shown on the GUI window. 




1 ,«,..„,.„ 




Thresh old 
131 


•1 1 •! | 




Camera Settings 

Brightness 
48 




^ M 


•1 1 -1 


Saturation 
40 


-1 1 -1 


20 


-1 1 •! 







Fig. 33. Developed software. 



When the scanning procedure is completed, the user can save images and relative robot 
position information in a file, save the cloud of points represent the surface of the test object 
and export the surface information in a file format that permits to load the data from the 
CAD software "CATIA". 
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Besides it's possible to load image information from a preview scanning procedure, this is 
useful for reconstruct the same laser path information using different calibration 
parameters. 

5.4 The surfaces reconstruction 

The system has been tested before in a fixed robot position, to verify calibration and 
reconstruction procedures, then the shape of some components, was defined using robot to 
move laser scanner module. The test objects are shown in the figure 34. 




Fig. 34. Test specimens. 



In the figure 35 and 36 it's possible to see a step of the procedure with the final results for 
the two test specimens. 
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Fig. 35. Elaboration procedure of the first test specimen 
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Fig. 36. Elaboration procedure of the second test specimen 
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By using the software CATIA it was possible to build the surface of the two test objects, in 
this way it was obtained the CAD model, this step of the 3D reconstruction method is a real 
reverse engineering application. The routine "Digitized shape editor" of the "CATIA" 
addresses digitalized data import, clean up, tessellation, cross sections, character line, shape 
and quality checking. In the figure 37 and 38 are shown the comparisons between the clouds 
of points and the respective surfaces for each object. 




Fig. 37. First test specimen results 




Fig. 38. Second test specimen results 



In the figures 39 and 40, an evaluation of 3D-reconstruction accuracy is shown for the two 
analyzed test specimen. It is possible to observe, that these first results have the worst 
accuracy along direction z of camera frame, according to observations of paragraph 5.3. 
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Fig. 39. First test specimen results 

The results of the 3D reconstruction obtained by means of the rig that was designed and 
developed at authors' laboratory were compared with the ones obtained by means of a 
commercial 3D laser scanner. In the figures 41 and 42 the clouds of points obtained with the 
two different rigs are compared. 
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Fig. 40. Second test specimen results 
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Fig. 41. Cloud of points obtained by the aouthors' robot assisted rig. 




Fig. 42. Clouds of point obtained with commercial laser scanner. 



In figure 43 is reported a comparison between the points obtained by the authors' rig and 
the commercial laser scanner. In figure 44 is reported a comparison between the surafces 
fobtaine by the above mentioned rigs. 
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Fig. 43. Comparison between results obtained with two different rig. 
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Fig. 44. Comparison between results obtained with two different rig. 



It was observed that in most cases the differences are no more than ±1.5 mm; just in few 
areas the differences can reach 5 mm. A more detailed analisys showed that these 
differences concern single points / so it is possible to presume that a preliminary analisyss of 
the cloud of points could permit a general increase of the reconstruction accuracy. 



548 Advances in Robot Manipulators 

6. Conclusions 

The proposed procedures are absolutely non-invasive since they do not involve any 
modification of the scene; in fact no markers with features visible by both the camera and 
the laser, or any other device, are required. 

As for the first results of the new method for real time shape acquisition by a laser scanner , 
it must be said that, although the test rig has been conceived just to validate the method 
(hence no high resolution cameras were adopted), the tests have showed encouraging 
results. These results can be summarized as follows. 

1. It is possible to calibrate the intrinsic parameters of the video system, the position 
of the image plane and the laser plane in a given frame, all in the same time. 

2. The surface shapes can be recognized and recorded with an appreciable accuracy. 

3. The proposed method can be used for robotic applications such as robotic 
kinematic calibration and 3D surfaces recognition and recording. For this last 
purpose the test rig was fitted on a robot arm that permitted to the scanner device 
to 'observe' the 3D object from different and known position. 

A detailed analysis of the sources of errors and the verification of the accuracy has been also 

carried on. As far as the latter aspect is concerned, the authors believe that a better system 

for tracking the position of the robot arm could enhance accuracy. 

Finally, the authors would like to point out that the solution proposed is relatively low cost, 

scalable and flexible. It is also suitable for applications other than RE, like robot control or 

inspection. 
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1. Introduction 

A good performance of robot control requires the consideration of efficient dynamic models 
and sophisticated control approaches. Traditionally, control law is designed based on a 
good understanding of system model and parameters. Thus, a detailed and correct model 
of a robot manipulator is needed for this approach [1, 2], 

A two-link planar nonlinear robotic system is a well-used robotic system, e.g., for welding in 
manufacturing and so on. Generally, a dynamic model can be derived from the general 
Lagrange equation method. The modeling of a two-link planar nonlinear robotic system 
with assumption of only masses in the two joints can be found in the literature, e.g., [3, 4]. 
Here, the authors revise centrifugal and Coriolis force matrix in the literature [3, 4] as 
pointed out in the next section. Furthermore, in practice, the robot arms have their mass 
distributed along their arms, not only masses in the joints as assumed. Thus, it is desired to 
develop a detailed model for two-link planar robotic systems with the mass distributed 
along the arms. Distributed mass along robot arms was discussed by inertia in SCAR A 
robot [5]. Here, we present a new detailed consideration of any mass distributions along 
robot arms in addition to the joint mass. 

Moreover, it is also necessary to consider numerous uncertainties in parameters and 
modeling. Thus, robust control, robust adaptive control and learning control become 
important when knowledge of the system is limited. We need robust stabilization of 
uncertain robotic systems and furthermore robust performance of these uncertain robotic 
systems. Robust stabilization problem of uncertain robotic control systems has been 
discussed in [1-3, 5-6] and many others. Also, adaptive control methods have been discussed 
in [1, 7] and many others. Because the closed-loop control system pole locations determine 
internal stability and dominate system performance, such as time responses for initial 
conditions, papers [6, 8] consider a robust pole clustering in vertical strip on the left half s- 
plane to consider robust stability degree and degree of coupling effects of a slow subsystem 
(dominant model) and the other fast subsystem (non-dominant model) in a two-time-scale 
system. A control design method to place the system poles robustly within a vertical strip 
has been discussed in [6, 8-10], especially [6] for robotic systems. However, as mentioned 
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above, there is a need of a detailed and practical two-link planar robotic system modeling 
with the practically distributed robotic arm mass for control. 

Therefore, this chapter develops a practical and detailed two-link planar robotic systems 
modeling and a robust control design for this kind of nonlinear robotic systems with 
uncertainties via the authors' developing robust control approach with both H=o disturbance 
rejection and robust pole clustering in a vertical strip. The design approach is based on the 
new developing two-link planar robotic system models, nonlinear control compensation, a 
linear quadratic regulator theory and Lyapunov stability theory. 

2. Modeling of Two-Link Robotic Systems 

The dynamics of a rigid revolute robot manipulator can be described as the following 
nonlinear differential equation [1, 2, 6, 10]: 

F c =M(q)q + V(q,q)q + N(q,q) (l.a) 

N(q,q) = G(q) + F d q + F s (q) (lb) 

where M(q) is an nxn inertial matrix, V(q,q) an n x n matrix containing centrifugal and 
coriolis terms, G(q) an n x 1 vector containing gravity terms, q(t) an n x 1 joint variable 
vector, F c an n x 1 vector of control input functions (torques, generalized forces), F d an 
n x n diagonal matrix of dynamic friction coefficients, and F s (q) an n x 1 Nixon static 

friction vector. 

However, the dynamics of the robotic system (1) in detail is needed for designing the 

control force, i.e., especially, what matrices M(q) , V(q, q) and G(q) are. 

Consider a general two-link planar robotic system in Fig. 1, where the system has its joint 

mass m l and m 2 of joints 1 and 2, respectively, robot arms mass m ir and m 2r distributed 

along arms 1 and 2 with their lengths /; and l 2 , generalized coordinates qy and q 2 , i.e., 

their rotation angles, q = [q\ q^\ ' > control torques (generalized forces) f l and f 2 , 

F c =Uxfil- 




Fig. 1. A two-link manipulator 
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Theorem 1. A general two-link planar robotic system has its dynamic model as in (1) with 

M(q) 



M n =(m l +^ l m rl +m 2 + m r2 )/j 

+ (m 2 + 4 2 m r2 )l 2 +2(m 2 +C2 m r2) l l l 2 cos l2 

M n =M 2l = (m 2 + £ 2 m, 2 )/ 2 2 

+ (m 2 +C 2 m r2 )l l l 2 cosq 2 

M 22 =(m 2 +4 2 m r2 )l 2 2 
V(q,q) = -(m 2 + C, 2 m r2 )l x l 2 q 2 sing 2 



2 1 
1 



G(q) = g 



(m, +4>! r l + '"2 +»V2)'l cos ?l + ( m 2 +^2 m r2) / 2 cos (?l +?2> 
(m 2 + £2 m r2)h cos (ll + 12) 



(2) 

(3.a) 

(3.b) 
(3.c) 

(4) 
(5) 



where g is the gravity acceleration, m l and m 2 are joints 1 and 2 mass, respectively, m rl 
and m r2 are total mass of arms 1 and 2, which are distributed along their arm lengths of /j 
and 1 2 , the scaling coefficients ^ , S, 2 , £j and £" 2 are defined as follows: 



4 i =$p i U)S i (l)l 2 dl/m ri l?, Ci=\^Pi{s)S i {l)lclllm ri l i , 
m ri =\ l j Pi (l)S i (!)dl,i = \,2 



(6) 



where pj (/) and P2 (0 are t ne arrn mass density functions along their length /, S\ (I) and 
S 2 (I) are the arm cross-sectional area functions along the length / . 

Proof: The proof is via Lagrange method and dynamic motion equations. The mass 
distribution can be various by introducing the above new scaling coefficients. Due to the 
page limit, detail of the proof is omitted. 

Remark 1. From (2)-(4) in Theorem 1, M(q) = V(q, q) . Theorem 1 is also different from the 
result in [3-6]. Especially, there are no corresponding items of £ \ in [3-6]. 
Corollary 1. A two-link planar robotic system with consideration of only joint mass has its 
dynamic model as in (1) and Theorem 1, but with 



0, 6=0, Ci =0, '=1,2 



(7) 



It means that its inertia matrix M(q) in (2), and 



M n =(m l +m 2 )li + m 2 l 2 +2m 2 hh cos ?2 > 
M \2 = M 2\ = m 2{ l 2+h l 2 co& l2)> M 22 = "l 2 l 2 



(8) 
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V(q,q) = -m 2 l 1 l 2 q 2 smq 2 



2 1 
1 



G{q) = g 



{my + m 2 )ly cosqy + m 2 l 2 C°s(<?i + <?2) 
m 2 l2COs(q l +q 2 ) 



(9) 
(10) 



Remark 2. It is noticed that centrifugal and Coriolis matrix V(q,q) in (26) is equivalent to: 



V(q,q) = m 2 l l l 2 smq 2 



qi -q\-qi 
<h 



(11) 



in (1). Note that the Coriolis matrix is different from some earlier literatures in [3, 4]. 
Theorem 2. Consider a two-link planar robotic system having its robot arms with uniform 
mass distribution along the arm length. Thus, its dynamic model is as (1) - (6) of Theorem 1 
with its scaling factors as follows: 



6-&«l/3 /a nd£i-S2-l/2 



(12) 



Proof: It can be proved by Theorem 1 and the uniform mass distribution in (6). 

Theorem 3. Consider a two-link planar robotic system having its robot arms with linear 

tapered-shapes respectively along the arm lengths as: 



n(l) = m -k t l ,0<l< 1/ , S,(l) = Hiif(f), i = 1,2 



(13) 



where r,- (/) in length is a general measure of the arm cross-section at the arm length I, e.g., 

as a radius for a disk, a side length for a square, -Jab for a rectangular with sides a and b, 

etc., Sj (I) is the cross-sectional area of arm i at its length position /, //,- is a constant, e.g., 

as 7t for a circle and 1 for a square. Assume that arm 1 and arm 2 respectively have their two 
end cross-sectional areas as: 

S i=S l (0) , S n = Si (h ),So2= S 2 (0) , S t2 = S 2 (l 2 ) (14) 

where Sqj > S t j , i = 1,2 . Their density functions are constants as Pj (I) = Pj , i = 1, 2 . Then, 
its dynamic model is as in (1) - (6) of Theorem 1 with its scaling factors: 



_ S 0i +6S ti +3 V'S'0|£»- _ % + 35 V/ + 2^jS iS t j 

ii ~ 1 1 Q i 



10(% + S tt + JS 0i S ti ) ' ' 4(% + S ti + JS 0i S ti ) 
?' = 1,2, and its arm mass: 



m ri = p, 



h(S 0i + S ti + ylS 0i S ti )/ 3, i = 1,2 



(15) 



(16) 
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Proof: It is proved by substituting (13) and (14) into (6) in Theorem 1 and further 

derivations. 

Remark 3. The scaling factors (15) and the arm mass (16) in Theorem 3 may have other 

equivalent formulas, not listed here due to the page limit. Here, we choose (15) and (16) 

because the two-end cross-sectional areas of each arm are easily found from the design 

parameters or measured in practice. The arm cross-sectional shapes can be general in (13) in 

Theorem 3. 

3. Robust Control 

In view of possible uncertainties, the terms in (1) can be decomposed without loss of any 
generality into two parts, i.e., one is known parts and another is unknown perturbed parts 
as follows [2, 6] : 

M = M +AM , N = N +AN, V = V +AV (17) 

where Mq, Nq, Vq are known parts, AM, AN, AV are 

unknown parts. Then, the models in Section 2 can be used not only for the total uncertain 

robotic systems with uncertain parameters, but also for a known part with their nominal 

parameters of the systems. 

Following our [6], we develop the torque control law as two parts as follows: 

F c =M (q)q d +V (q,q)q + N (q,q)-M (q)u (18) 

where the first part consists of the first three terms in the right side of (18), the second part is 
the term of w that is to be designed for the desired disturbance rejection and pole clustering, 
q d is the desired trajectory of q, however, the coefficient matrices are as (2) - (6) in Theorem 

1 with all nominal parameters of the system. Define an error between the desired q d and 
the actual q as: 

e = q d -q. (19) 

From (1) and (17)-(19), it yields: 

e = M~\q)[AM{q\q d +AV(q,q)q + AN(q,q) + M (q)u] 
= w + Ee + Fu + u (20) 

E = -M~\q)AV{q,q), F = -M~\q)AM{q) 

w = -Fq d - Eq d +M~ l AN (21) 

From [6], we can have the fact that their norms are bounded: 

|w|<<y w , ||£|<<? e , \\F\\<S f (22) 

Then, it leads to the state space equation as: 
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x = Ax + Bu+B[0 E]x + BFu + Bw 



= [«j e 2 e x e 2 ]', A- 



~0 I 


, B = 





Oj 




7 



(23) 
(24) 



The last three terms denote the total uncertainties in the system. The desired trajectory q d 
for manipulators to follow is to be bounded functions of time. Its corresponding velocity q d 
and acceleration q d , as well as itself q d , are assumed to be within the physical and 

kinematic limits of manipulators. They may be conveniently generated by a model of the 
type: 

q d (t) + K v q d (t)+K p q d (t) = r(t) (25) 

where r(t) is a 2-dimensional driving signal and the matrices K v and K p are stable. 
The design objective is to develop a state feedback control law for control u in (18) as 

u{t) = -Kx{t) (26) 

such that the closed-loop system: 

x = (A-BK + B[0 E]-BFK)x + Bw (27) 

has its poles robustly lie within a vertical strip L2 : 

A(A C ) e Q = {s = x + jy\- a 2 < x < -a x < 0} (28) 

and a 8-degree disturbance rejection from the disturbance w to the state x, i.e., 

||r OT (^| oo =|(^-4)- 1 J BL<(5 (29) 

A c =A-BK + B[0 E]-BFK (30) 

From [6], we derive the following robust control law to achieve this objective. 
Theorem 4. Consider a given robotic manipulator uncertain system (27) with (l)-(6), (17)- 
(22), (24), where the unstructured perturbations in (21) with the norm bounds in (22), the 
disturbance rejection index 8 > in (29), the vertical strip fi in (28) and a matrix Q>0. 
With the selection of the adjustable scalars S\ and £ 2 > i- e -> 



{\-S f )IS e >£ x >0, (\-d f -£ x S e )S>e 2 >0 
there always exists a matrix P > satisfying the following Riccati equation: 



(31) 



ROBUST CONTROL DESIGN FOR TWO-LINK NONLINEAR ROBOTIC SYSTEM 



557 



A' al P + PA al - (1 - 8 f - s x S e - e 2 I S)PBB'P 



where 



A + a x I ■■ 



a x I 2 



(32) 



(33) 



Then, a robust pole-clustering and disturbance rejection control law in (18) and (26) to 
satisfy (29) and (30) for all admissible perturbations E and F in (22) is as: 



u = -Kx = -rB'Px 
if the gain parameter r satisfies the following two conditions: 



(ii) 



(i) r > 0.5 and 

2a 2 P + A'P + PA-(S e ls\)I 
- [2r(l + S f ) + e l S e ]PBB 'P>0 



(34) 

(35) 
(36) 



Proof: Please refer to the approach developed in [6, 8] and utilizing the new model in 

Section 2. 

It is also noticed that: 

"0 0" 
/, 



BB' : 



(37) 



It is evident that condition (i) is for the a,\ -degree stability and S -degree disturbance 
rejection, and condition (ii) is for the a 2 -degree decay, i.e., the left vertical bound of the 
robust pole-clustering. 

Remark 4. There is always a solution for relative stability and disturbance rejection in the 
sense of above discussion. It is because the Riccati equation (32) guarantees a positive 
definite solution matrix P, and thus there exists a Lyapunov function to guarantee the robust 
stability of the closed loop uncertain robotic systems. The nonlinear compensation part in 
(18) has a similar function to a feedback linearization. The feature differences of the 
proposed method from other methods are the new nominal model, and the robust pole- 
clustering and disturbance attenuation for the whole uncertain system family. It is further 
noticed that the robustly controlled system may have a good Bode plot for the whole 
frequency range in view of Theorem 4, inequality (29) and its H-infinity norm upper bound. 
Remark 5. The tighter robust pole-clustering vertical strip —a.\ > Re 2-(A c ) > -a 2 nas 



-1/2 



a 2 =«] + 0.5/1 {P~ uz [-A'P-PA a i +(8 e ls x )I 



1/2, 



+ (2r(l + 8 f ) + e x 8 e )PBBP\P~ llL } 



(38) 
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4. Examples 

Example 1. Consider a two-link planar manipulator example (Fig. 1). First, only joint link 
masses are considered for simplicity, as the one in [3, 6]. However, we take the correct 
model in Corollary 1 and Remark 2 into account. The system parameters are: link mass 
mi = 2kg, m 2 =10Ag, lengths /] = \m , / 2 = \m , angular positions c\x, qi (rad), applied 
torques f\, J2 (Nm). Thus, the nominal values of coefficient matrices for the dynamic 
equation (1) in Corollary 1 are: 



M (q)- 



N(){q,q)- 



,V (q,q) = - 


\-0S 2 q 2 


"2 1 
1 


"l2Ci+10C 12 ~ 




10C 12 







(39) 



22+20C 2 io(i + c 2 ;T 
10(1 + c 2 ) 10 

N 

where C, = cos q t , i = 1, 2 , Cj 2 = cos(qi + <? 2 ) , S2 = sin q 2 , and g is the gravity accelera- 
tion. 
The desired trajectory is q^it) in (25) with K v =0, and 

K p = I , the signal r(t) = [0.5 lj , the initial values of the desired states q d (0) = [2 2\ , 
q d (0) = 0,i.e., 

q dl (t) = l.5cost + 0.5, and q d2 (t) = cost + \ (40) 



The initial states are set as ^i(0) = g 2 (0) = -2 , and ^i(0) = ^ 2 (0) = 0, i.e., ej(0)=4, 
e 2 (0) = 4, e[(0) = 0,and e 2 (0) = 0. The state variable is x = [e' e']' where e = q d —q. 
The parametric uncertainties are assumed to satisfy (22) with 8 r = 0.5 , S e = 40 , S N = 10 . 
Select the adjustable parameters e\ =0.012,£ 2 =0.0015 from (31), disturbance rejection 
index 8 = 0.1, the relative stability index <X\ =0.1, and the left bound of vertical strip 
a 2 = 2000 since we want a fast response. By Theorem 4, we solve the Riccati equation (32) 
to get the solution matrix P and the gain matrix as: 



12693/ 2 1584/ 2 
1584/ 2 1643/ 2 



,K = rB'P = [950 A&23I 2 985.7863/ 2 



with r = 0.6. The eigenvalues of the closed-loop main system matrix A—BK are 
{-0.9648, -0.9648, -984.8215, -984.8215). Remark 5 gives the result -« 2 =-1873. The 

uncertain closed-loop system has its -a 2 < Re[/l(^4 c )] < -aj robustly. 
The total control input (law) is 



: M q d + Vo (?, q) q + n - M u 
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+ lOS 2 q 2 



c ll 



1.5cos? 
- cos? 

12Q + 10C 12 
10C 12 



22 + 20C 2 10(1 + C 2 ) 
10(1 + C 2 ) 10 

■2-1 ,y, 
1 

22 + 20C 2 10(1 + C 2 )~ 
10(1 + C 2 ) 10 

A simulation for this example is taken with AM(q) = 0.4M (g) , i.e., 40% disturbance, 

|m _1 (^)AA/(9)| = 0.2857 < S f =0.5, AV m (q,q) =0.2V m0 (q,q) with 20% disturbance, and 

AN(q) = 0.2N Q (q) with 20% disturbance. The simulation results by MATLAB and Simulink 
are shown in Figs. 2-3. 



[950.1823/ 2 985.7863/ 2 ] 



(41) 




Fig. 2. States and their desired states: (a) q\(t) & q\d(t) , (b) q 2 (t) & q 2l ](t) in Example 1 
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5 10 15 20 25 30 



Fig. 3. Error signals: (a) ei(t), (b) 62(f) in Example 1 

Example 2. Consider a two-link planar robotic system example with the joint mass and the 
arm mass along the arm length. The mass of joint 1 is ;kj = 1 kg, and the mass of joint 2 is 

mj = 0.5 kg. The dimensions of two robot arms are linearly reduced round rods. The two 

terminal radii of the arm rod 1 are r 01 = 3 cm and r (1 = 2 cm. The two terminal radii of the 

arm rod 2 are r§2 = 2 cm and r t \ = 1 cm. Their end cross-sectional areas are Sqi = 9rt cm 2 , 

S t i = An cm 2 , 5"o2 = An cm 2 , and S t 2 = \7t cm 2 . The arm length and mass are /j = 1 m, 

I2 = 1 m, m r \ = 5.2 kg, and m r 2 =1.9 kg. By Theorem 3, it leads to: 



£ = 0.2684 , £, = 0.2286 , Ci = 0.4342 , C 2 = °-3929 
By Theorem 3, the nominal model parameters are: 



(42) 



M (q) 



5.7301 + 2.4929C, 0.9343 + 1.2464C- 



0.9343 + 1.2464C 2 
N (q,q) 



0.9343 
5.6579C 



V (q,q)= -l.2464S 2 q 2 



2 1 
1 



1.2464C, 



12 



1.2464Q2 



(43) 
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The desired trajectory qj(f) is the same as in Example 1. The initial states are set as 
qi(0) = -2, <? 2 (0) = 0, <h(0)= <j 2 (0) = / i.e.,e 1 (0) = 4, e 2 (0) = 2, e 1 (0) = 0,e 2 (0) = . 
The parametric uncertainties in practice are assumed to satisfy (22) with 8i =0.25, 
8 e =10, Sjq =10. Select the adjustable parameters E\ =0.0375 and e 2 =0.0188 from 
(31), the disturbance rejection index 8 = 0.1 , the relative stability index ay = 0.1 , and the 
left bound of vertical strip a 2 = 100 . By Theorem 4, the solution matrix P to (32) and the 
gain matrix K are 

"898.87/ 2 13.55/ 2 
13.55/ 2 12.47/ 2 

with r = 4.8 . The eigenvalues of the closed-loop main system matrix A — BK are 
{-1.1072,-58.7587, -1.1072,-58.7587}. The uncertain system has 

-a 2 < Re[/t(^4 c .)] < -a; robustly. 
The total control input (law) is : 



, K = rB'P = [65.0589i 2 59.8659i 2 ] 



f = M q d +V (q,q)q + N -M u 
5.7301 + 2.4929C 2 0.9343 + 1.2464C 2 1 ["-1.5cos? 
0.9343 + 1.2464C 2 0.9343 |_ -cos? 



1.24645 2 g : 



2 1 
1 



5.6579Ci+1.2464C 12 
1.2464C 12 



5.7301 + 2.4929C 2 0.9343 + 1.2464C 2 
0.9343 + 1.2464C 2 0.9343 

■[65.0589/ 2 59.8659/ 2 ]- [e' e'f 



(44) 



The simulation is taken with AM(q) = 0.25Afg(g) , AV m (q, q) = 0.1K m0 (g, q) , and 
AN(q) = O.lNQ(q) . The results are shown in Figs. 4-5. It is noticed that the error may be 
reduced when the gain parameter r is set large. 



q1 and q1 -desired 
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q2 and q2-desired 




Fig. 4. States and their desired states: (a) q\(t) & qici(t),(b) ^2(0 & <72d(0 

el 
4 
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-0.5 
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Fig. 5. Error signals: (a) 62(f)' & 0°) e 2(t) 
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5. Conclusion 

The chapter develops the practical models of two-link planar nonlinear robotic systems with 
their arm distributed mass in addition to the joint-end mass. The new scaling coefficients 
are introduced for solving this problem with the distributed mass along the arms. In 
addition, Theorems 2 and 3 respectively present two special cases: a uniform arm shape (i.e., 
uniform distributed mass) and a linear reduction of arm shape along the arm length. 
Based on the presented new models, an approach to design a continuous nonlinear control 
law with a linear state-feedback control for the two-link planar robotic uncertain nonlinear 
systems is presented in Theorem 4. The designed closed-loop systems possess the 
properties of robust pole-clustering within a vertical strip on the left half s-plane and 
disturbance rejection with an H x -norm constraint. The suggested robust control for the 

uncertain nonlinear robotic systems can guarantee the required robust stability and 
performance in face of parameter errors, state-dependent perturbations, unknown 
parameters, frictions, load variation and disturbances for all allowed uncertainties in (22). 
The presented robust control does always exist as pointed out in Remark 4. The adjustable 
scalars e t , i=l, 2, provide some flexibility in finding a solution of the algebraic Riccati 
equation. The designed uncertain system has a,\ -degree robust stabilization and 8 -degree 
disturbance rejection. The controller gain parameter r is selected such that the designed 
uncertain linear system achieves robust pole-clustering within a vertical strip. The examples 
illustrate excellent results. This design procedure may be used for designing other control 
systems, modeling, and simulation. 
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1. Introduction 



Simulation of robot manipulator in Matlab/Simulink or any other mechanism simulator is 
very common for robot design. However, all these approaches are mainly concerned with 
design configuration having little analysis meaning that the robot model is formed by 
linking the kinematics and solid description, and simulated for alternative configuration of 
movements (Cleery & Mathur, 2008). Indeed comprehensive design should have analysis at 
different computational levels. Finite element method (FEM) has been a major tool to 
develop a computational model in various fields of studies because of its modelling and 
simulation capability close to reality. Subsequently, modelling and analysis with FEM has 
become the most convenient way to economically design and analyze real world problems, 
either in static or dynamic. As a result, huge amount of reports on this topic can be found in 
the literature (Mackerle, 1999). Unfortunately however, this technique has not 
comprehensively applied in designing in designing a robot while choosing the best 
components for the design is as important as having good performance and no 
environmental impact of the machines over its lifetime. Building block of a robot 
manipulator is electromechanical system in which mechanical systems are controlled by 
sophisticated electric motor drives. Since energy saving everywhere is a major challenge 
now and in future, getting electromechanical design right will significantly contribute to 
energy saving. 

This chapter is dedicated to the application of Finite Element Method (FEM) in designing 
multi-axes positioning for robot manipulators. Computational model that can predict 
physical behaviour of dynamic robot manipulators constructed using FE codes is presented, 
and this is major contribution of the chapter. FEM tools necessary for modelling and 
analysis of multi-axes positioning are presented in large part. Rather than a FEM discourse, 
FEM is presented by highlighting mathematics behind and an application example as they 
relates to practical robotic manipulation. It is, however, assumed that the reader has 
acquired some basic knowledge of FEM consistent with the expected level of mathematics. 
Hence, the chapter is organized as follows. 

In the early part of the chapter, the important terminologies used in robotics are defined in 
the background. The material is presented using a number of examples as evidenced in the 
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published reports. Then the chapter will go to mathematics behind finite element modelling 
and analysis of kinematics of a structure in 3D space as robotics involves tracking moving 
objects in 3D space. This will also include mathematical tools essential for the study of 
robotics, particularly matrix transforms, mathematical models of robot manipulators, direct 
kinematic equations, inverse kinematic technique and Jacobian matrix needed to control 
position and motion of a robot manipulator. More emphasis will be on how these 
mathematical tools can be linked to and incorporated into FEM to carry out design analysis 
of robot structure. 

The rest of the chapter will present application of FEM in practical robot design, detailed 
development of FE model computable for multi-axes positioning using a particular FE code 
(ALGOR, 2008), and useful results predicted by the computational model. The chapter will 
be closed by concluding remarks to choice of FE codes and its impact on the computational 
model and finally the usefulness of computational model. 

2. Background in Robotics 

Multi-axis positioning meant here is different movements of a point, or a structure in 
different directions. This term is drawn from the term usually come with computer 
numerical controlled machines just as 3-axis, 5-axis and so on, where the 3-axis machine, for 
instance, implies that it can make a maximum of three different positioning of the controlled 
elements. Each axis is alternatively referred to as degree of freedom (DOF) that is something 
to do with motion in a system or a structure. Since the term 'axis' is adopted to represent an 
element that creates motion, 3-axis positioning means three DOF's, for example (Rahman, 
2004). In relation to these definitions, one manipulator of a robot can represent one axis or 
DOF as the manipulator is the robot's arm, a movable mechanical unit comprising of 
segments or links jointed together with axes capable of motion in various directions 
allowing the robot to perform tasks. Typically, the body, arm and wrist are components of 
manipulators. Movements between the various components of the manipulator are 
provided by series of joints. 

The points that a manipulator bends, slides or rotates are called joints or position axes. 
Position axes are also called the world coordinates. The world coordinate system is usually 
identified as being a fixed location within the manipulator that serves as an absolute frame 
of reference. 

In general, the manipulator's motion can be divided into two categories: translation and 
rotation. Although one can further categorize it in specific term such as a pitch (up-and- 
down motion); a yaw (side-to-side motion); and a roll (rotating motion), any of these is fall 
into either translation or rotation. The individual joint motion associated with either of these 
two categories is referred to degree of freedom. Subsequently, one degree of freedom is 
equal to one axis. The industrial robots are typically equipped with 4-6 axes. 
The power supply provides the energy required for a robot to be operated. Electricity is the 
most common source of power and is used extensively with industrial robots. Pay load is the 
weight that the robot is designed to lift, hold, and position repeatedly with the same 
accuracy. Hence, the power supply has direct relation to the payload rating of a robot. 
Among the important dynamic properties of a robot that properly regulates its motion are: 
stability, control resolution, spatial resolution, accuracy, repeatability and compliance. To 
take these factors into account in the design of a robot is a complex issue. Lack of stability 
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occurs very often due to wear of manipulator components, movement longer than the 
intended, longer time to reach and overshooting of position. 

Control resolution is all about position control. It is a function of the design of robot control 
system and specifies the smallest increment of motion by which the system can divide its 
working space. It is the smallest incremental change in position that its control system can 
measure. In other words, it is the controller's ability to divide the total range of movements 
for the particular joint into individual increments that can be addressed in the controller. 
This depends on the bit storage capacity in the control memory. For example, a robot with 8 
bits of storage can divide the range into 256 discrete positions. The control resolution would 
be also defined as the total motion range divided by the number of increments. For example, 
a robot has one sliding joint with a full range of 1.0 m. The robot control memory has 12-bit 
storage capacity. The control resolution for this axis of motion is 0.244mm. The spatial 
resolution of a robot is the smallest increment of movement into which the robot can divide 
its work volume. 

Mechanical inaccuracies in the robot's links and joint components and its feedback 
measurements system (if it is a servo-controlled robot) constitute the other factor that 
contributes to spatial resolution. Mechanical inaccuracies come from elastic deflection in the 
structural members, gear backlash, stretching of pulley cords and other imperfections in the 
mechanical system. These inaccuracies tend to be worse for large robot simply because the 
errors are magnified by the large components. The spatial resolution is degraded by these 
mechanical inaccuracies. 

Rigidity of the structure also affects the repeatability of the robot. Compliance is a quality 
that gives a manipulator of a robot the ability to tolerate misalignment of mating parts. It is 
essential for assembly of close-fitting parts. In an electric manipulator, the motors generally 
connect to mechanical coupling. The sticking and sliding friction in such a coupling can 
cause a strange effect on the compliance, in particular, being back-drivable. 
An Off-line programming system includes a spatial representation of solids and their 
graphical interpretation, automatic collision detection, incorporation of kinematic, path 
planning and dynamic simulation and concurrent programming. The off-line programming 
will grow more in the future because of graphical computer simulation used to validate 
program development. It is important both as aids in programming industrial automation 
and as platforms for robotic research (Billingsley, 1985; Keramas, 1999; Angeles, 2003). 

3. Mathematical Foundation 

Forward and reverse kinematics methods are the principal mathematics behind typical 
modeling, computation and analysis of robot manipulators. Since the latter is deduced from 
the former, broader review will focus on some related mathematics of the former. Forward 
kinematic equation relates a pose element to the joint variables. The pose matrix is 
computed from the joint variables. The position and orientation of end-manipulator (also- 
called the end-effector, the last joint that directly touches and handles the object) is 
computed from all joint variables. The position and orientation of the end manipulator are 
computed from a set of joint variable values which are already known or specified. The 
computation follows the arrow directions starting from joint 1 as illustrated in Fig. 1. It 
should be noted that in kinematics analysis, the manipulators are assumed to be rigid. 



568 



Advances in Robot Manipulators 



3.1 Defining the location of an object in space 

As a general case, the object location in 3D space is considered. A matrix representation is 
widely used to represent the object location as it is convenient and easy to handle especially 
when the location is changed. Two parameters are needed to define the object location: 
position and orientation. Basically, a homogenous vector 'v' is represented as 



if v is a free vector or 



(1) 



(2) 



if v represents the position of a particular point in the usual coordinates system (x, y, and z). 
This coordinates system is referred to a frame. These frames are used to track an object 
location in space. As shown in Fig. 2, the frame F is attached to a fixed point while another 
Fa to an object. The object position is described by the vector pA of the origin A of the frame 
Fa. The orientation of the object is given by the homogeneous vectors of each unit vectors 
xa, yA and za of Fa with respect to F . 
Then the object location is mathematically represented by a post matrix 'P' as: 



p = [ x a y A z a Pa] 
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formed by the coordinates of the vectors xA, yA and zA is a rotation matrix that holds the 
orientation of the object while the pa holds the position of the object. In a compact form, the 
pose matrix can be written as: 
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where O refers to a 1x3 vector of zeros. 




Fig. 1. Forward kinematics 
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,, -^ _*/A mobile object 
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Fig. 2. The object with respect to frames. 



Fo : fixed frame 

F A : mobile object frame 



When a point, say Q given by its coordinate vector A q 






with respect to frame Fa, is 



transformed to the frame Fo, the transformed vector say °q can be expressed as: 
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In compact form 
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Similarly in alternative representation of frame transforms, simple space translation and 
rotation can be conveniently represented as a compact- form matrix as: 



T q = T u q 

Rq = R v ,e q 



(9) 
(10) 



where q 
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y q 



is coordinates vector of a point Q, T u is a translation vector, R v ,e is a rotation 



vector, T q is coordinates vector of a point Q 1 where Q is translated by T u and R q coordinates 
vector of a point Q 1 where Q is rotated around v by an angle 0. 

Furthermore, the translations and rotations along the reference axes, called canonical 
translations/ rotations, have the homogeneous matrix of the following form respectively as: 
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Where T x [Ci), T^ {(£), aild 7^. (dj are translations of a distance, 'd' in x, y and z directions 
respectively, and R x [(x) r R.y{p) ! and fi^^'J are rotations about the respective axis by 
the respective angle with Ct\ — COS ^ J and S/\ = Sl7l\ ). These can be expanded to 
arbitrary transformations. Refer to Manseur (2006) for details. 

3.2 DH parameters 

In the conventional analysis of motion of robot manipulators, the Denavit-Hartenberg (DH) 
modelling technique is commonly used as a standard technique. Reference frames are 
assigned to each link based on DH parameters, starting from the fixed link all the way to the 
last link. The DH model is obtained by describing each link frame with respect to the 
preceding link frame. The original representation of one frame with respect to another using 
pose matrix requires a minimum of six parameters. The DH modelling technique reduces 
these parameters to four, routinely noted as: 

di, the link offset, 

ai, the link length, 

8i, the link angle and 

ai, the link twist as illustrated in Fig. 3. 




Fi-1 



Fig. 3. Illustration of DH parameters 
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Referring to Fig. 4, in relation to the link frames and DH parameters, homogenous frame 
transform from the frame (i) to (i-1) can be generally written as 



iA 1 = r(z J d i ) ff(z,d { ) IfoaJ R(x,a t ) 



(17) 



where T(z, di ) is translation from Fi-1 to Fd, R(z, 0i ) the rotation transform from Fd to F0, 
T(x, ai ) the translation transform from F0 to Fa, R(x, ai) the rotation transform from Fa to 
Fi. 




Fig. 4. Frame transformation of the DH parameters. 



□ Fi 

■ Fi-1 

■ Fd 

■ F0 

□ ^ 



The matrix HAi is then used to express the transform of the end frame to the base frame for 
'n-axis' or 'n-joint' robot manipulators. That is 



D u = A ± A 2 
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(18) 
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where T n = A-^A^ ^n-1 n = = P ose rnatrix of the end-effector. This process is 

called forward kinematic in modelling, computation and analysis of robotic manipulator. 
In the reverse kinematics method, as it means, one or more sets of joint variables are 
computed from the known end-effector pose matrix. As such the direction of computation is 
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revered from that of the forward kinematics as illustrated in Fig. 5. Nevertheless, the same 
basic concept of forward kinematics is applied except the fact that the mathematical 
manipulation becomes different and sophisticated. To be brief, the equations derived from 
the forward kinematics method are reduced to a set of equations involving as few unknown 
joint variables as possible using a kinematic function that depends on only certain joint 
variables. This results simplified set of equation that can be solved analytically. Details can 
be found elsewhere (Manseur, 2006). 




Fig. 5. Reverse kinematics 

3.3 Velocity Kinematics 

When the time factor is taken into account in the movement of a manipulator, Jacobian 
matrix is used to manipulate the transformation of manipulator. The generalized velocity 
vector V of the end-effector with respect to the base frame can be a form of a linear velocity 
vector v and an angular velocity vector co: 



v = 



(20) 



where 
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For the case of n-joint robot, the relationship between the V and the joint velocities can 
generally be expressed as 



V=j(q)q = 



h 

J LJ- 



(23) 



Whereq=['?i ?2 Qn] is the joint variable and if =[ C] j ^2 (j n J J 

is corresponding velocity vector. The matrix J(q) is called the Jacobian matrix. It is the 
mathematical relationship between joint motion and end-effector motion. It plays an 
important role in the analysis and control of robotic motion owing to the fact that it 
establishes a linear relation between velocity vectors in Cartesian space and in joint space. 
Details of derivation of J and its computation can be found in (Manseur, 2006). 

3.4 Typical Robot Simulators 

Nowadays softwares are readily available to generate and solve the kinematic equations. 
BUILD, SKEG and SIMULINK are some of those. Typical robot simulators allow the 
programming of a manipulator positioning in Cartesian space and relate a Cartesian co- 
ordinate set to the robot model's joint angles known as the inverse kinematics 
transformation (Zlajpah, 2008). 

Kinematic description is made up of joint rotation and position information. The inverse 
kinematic links the kinematics and solid descriptions. During the linking process, a 
sequence of procedure is called to solve a particular position of the robot. For example, the 
method of solving 6 DOFs robot as shown in Fig. 6 requires the following steps: 

i. From tool position and orientation, calculate wrist position 

ii. Calculate angle of rotation of axis 1 

iii. Calculate angle 2 and 3 from the two link mechanism connecting the 

shoulder and the wrist 

iv. Calculate the remaining joint angles using spherical trigonometry [1] 

Robot simulators do not allow dynamic effects or systematic errors in either trajectory 
calculation or inverse kinematic processing. The implementation of a full dynamic model 
would be out of question for a general simulator (e.g. BUILD simulator). 
Robot errors can be categorized into two: static error related to end points and dynamic 
error related to path. The formers result from deviation of the achieved position from the 
demand position. The latter comes from mismatch between a desired path and the true path. 
The computation of such information is very appropriate for a solid modelling system in 
which methods for analyzing space are well developed. However in these simulations, there 
will be differences between the actual robot performance and the modelled robot 
performance. The difference will depend on the assumptions made when designing robot 
model. 

A particular robot's physical kinematics will deviate from the idealized model due to 
manufacturing tolerances and this will lead to a reduction in the accuracy of the robot's 
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positioning with respect to a Cartesian space. The weight affects inaccuracies due to 
compliance of robot manipulators (Billingsley, 1985). 





Fig. 6. Robot with 6 DOFs. 

To sum up, in conventional modelling technique, the robot manipulators are modelled as a 
kinematic model. The accuracy of simulated motion in this technique is questionable. 
Additionally, the model is inadequate for the analysis of the dynamics laws of motion in 
which inertial parameters, link masses and shapes, and applied forces and torques must be 
considered. This is where FEM comes into play. 



4. Finite Element Method 

It is hard to completely define finite element method (FEM) in one sentence or even a 

paragraph. One may define it as a numerical method for solving engineering problem and 

physics, or a method to computationally model reality in a mathematical form; either one is 

acceptable indeed. However, for more complete definition of FEM, the following paragraph 

may suit it better. 

"A continuum is discretized into simple geometric shapes called finite elements; constitutive 

relations, loading and constraints are defined over these elements; assembly of elements 

results set of equations; solution of these equations gives the approximate behaviour of the 

continuum." 

The procedure of using FEM to analyze the system behaviour generally includes: 
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Discretize the system and select element type. 

Select displacement function. 

Define strain-displacement and stress-strain relation. 

Derive stiffness matrix and equations. 

Assemble global equations. 

Introduce boundary condition (BC) 

Solve for unknown variables. 



Finite element method (FEM) is now considered matured to develop a computational model 
to predict solution domain accurately without the need of expensive experimental work. 
Some advantages of using FEM are being economically-viable technique, flexibility to 
analyze various physical problems, and capability of modelling complex boundary 
conditions and material behaviour. Most importantly, the aspects of FEM applications are 
easily adaptable to engineers' needs (Chandrupatla, & Belegundu, 2002; Hutton, 2004). . 

4.1 Multi-axes positioning with FEM 

Multi-axis positioning in FEM can be modelled with manipulation of parameters that can be 
found under the category of boundary conditions. The concept of DOF here can be 
illustrated in a simple spring-mass system. Fig. 7(a) shows a system consisting of one spring 
and one mass where one DOF is imposed on the mass as it displaces due to the weight or 
applied force. When there is more than one spring or mass, DOF also becomes more than 
one as shown in Fig. 7(b). In both cases, DOF is in translation. In general structural 
dynamics, motion can be in translation or rotation defined in standard coordinate axes, x, y 
or z. These motions are illustrated in Fig. 7(c) where Ux, Uy, and Uz are translations in x, y 
and z directions while Rx, Ry, and Rz are rotations about x, y and z axes respectively. 
Hence, in any general-purpose finite element (FE) code, maximum of six DOFs can be 
defined at a node point for structural analysis. 

4.2 Structural analysis with FEM 

Basically, computation in FEM involves developing matrix equations for the system and 
manipulation of these equations. Therefore, one may say that the heart of FEM is rooted in 
matrix manipulation. For simplest static problem shown in Fig. 8 the system is discretized 
into two spring elements and three nodes. The FE equation for this system with reference to 
the coordinate axes can be derived as: 
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Where K^and K^axe spring constants, £/-p li-^s 3HQ t/ 3 are displacements or DOFs at 
nodes, and F-y s F„ , and r 3 are applied forces. 
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Fig. 7. Motion in standard coordinate axes 
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Fig. 8. FE model with spring elements 



Detail of derivation procedure is available in (Hutton, 2004). In order to solve for any 
unknown in equation 24, all possibly-known BC's and forces are imposed. The matrix 
equations are then solved using an appropriate solver. Thus in FEM, the coordinate axes can 
be defined up to the user, the kinematics and physics of the system are taken care of in BC 
and material model respectiely, and all necessary computations are performed by the solver. 
The FE equations are becoming complicated and the more advanced mathematics is 
required to solve when the system is in dynamics and constructed of complicated geometry 
in 3D where the element geometry is not so simple as the above example, and also number 
of elements can go to thousands. For dynamics analysis, finite element formulation is 
usually based on Lagrangian mechanics which states: 
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L = T 



(25) 



where L is Lagrangian term and T is total kinetic energy of the bod. In dynamic problem, 
mass and acceleration effects come into picture. For 3D body, T can be expressed as: 



T = — \\\(u 2 +v 2 + w 3 )p dx dy dz 



(26) 



Where p is mass density of the body, and u ',v V'and" w ' are velocity components of 
differential mass associated with displacements (u, v, w) in the coordinate directions. If 
computational domain is discretized by 



u(x, y, z, t) = Y J N i (x, y, z)u t (t) 
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w(x, y, z, t) = Y J N i (x, y, z)w,.(0 



the element kinetic energy can then be expressed as 
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In compact form, 



4m 



{u} T [N] T [N]{u} + W T {N} T [N]^ 
+ {w} T [Nf[N]{w}pdV (e} 



T M =^{S} T [m M ]{q} 



being nodal velocities as 
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and the element mass matrix as 
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Then based on the Lagrangian approach, the global equations for the FE model of a 
structure subjected to dynamic loading can be expressed in the form 
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where P is the total number of DOFs. Solving these equations yields a general system of 
ordinary equation: 

[M]{q} + [K]{q} = {F} (35) 

Finite element solver will solve equation (35) for displacements thereby strains and stresses. 



5. Application Example 

Application of FEM in modelling and analysis of multi-axis positioning is demonstrated in 
this section. The real robot manipulators (model: Fanuc M-6iB) shown in Fig. 9 is used as a 
reference for realistic manipulator geometry. Fig. 10 shows the number of axes of the 
reference robot. The axes were assigned as Jl, J2, and so on. All axes are in rotation. This Fig. 
was used for the demonstration purpose of the axes. The actual rotation of each axis can be 
found in (Fanuc Robotics, 2008). Modelled robot geometry was simplified and designed in 
Solidworks as shown in Fig. 11 keeping the important features of the reference robot 
manipulators. Despite simplification to manipulator geometries for physical analysis, 
motions of each manipulator were identical to the real robot, except the fact that virtual 
robot would have one DOF i.e. J6 less. The dimensions were based on approximate scale of 
the reference robot. Small holes were created in each component and used as reference 
points for assembly into complete robot geometry. These holes would also be useful for 
defining joints in finite element analysis. 

As shown in Fig. 11, the virtual robot has five axes. Each axis would be called Jl, J2, and so 
on just as assigning in the reference robot specification. Solidworks model was saved as the 
IGES file format that can be exported to finite element package for simulation of physical 
behaviour. 
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Fig. 9. Reference robot - model FANUC M-61B (Fanuc Robotics, 2008). 







Fig. 10. The axes of the reference robot manipulators (Fanuc Robotics, 2008) 
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Fig. 11. Simplified robot model geometry 

5.1 Finite element analysis 

The model geometry of robot manipulators was imported from Solidworks into finite 
element environment for physical analysis. General-purpose FE codes (ALGOR version 21.2) 
were tested for the analysis. Fig. 12 shows the discretized model where 8-node brick 
elements were used as element type for all components. Total number of elements used was 
20875 brick elements in manipulators (Jl - J5) and 736 beam elements at joints. According to 
the available information, steel material (AISI 4130) was used for J5 and the base and the 
remaining parts were made of aluminum 2024-T3. Linear material model was assumed and 
the properties of these materials were modelled with data available in FE package material 
library. Element formulations were solved for large displacement using total Lagrangian 
approach. 

In order to define five axes in the manipulators, joint mesh method and beam elements were 
used. Pin joint type was chosen for relative movement of each manipulator. Beam elements 
were selected for joint mesh as these elements can withstand the moment induced. 
Boundary conditions were then defined at each joint of the manipulator using local 
coordinate system assigned at joints to create motion being consistent with the reference 
robot. As this type of robot has to be fixed on the floor, at ceiling or on the table when in use, 
all nodes at the base were constrained in all directions. To rotate manipulator Jl relative to 
J2, nodal rotation about Z-axis was prescribed at its joint. At the same time, J2 is rotating 
about its own axis X as well as swinging about Z. The former DOF is defined by assigning 
initial velocity at corresponding joint and the latter by using rotational DOF at the node 
joining with J4. Similarly, the remaining DOFs were defined to create motions of other 
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manipulators. To eliminate the unknowns in solving FE equations, additional nodal and 
surface boundary conditions were applied on all joint node and surfaces of manipulators 
respectively in such a way that translation or rotation in unnecessary directions were 
constrained. 

Mechanical event simulation (MES) with linear material model was used to analyze the 
kinematics as well as physical response due to dynamic effects. MES can simultaneously 
analyze mechanical behaviour involving large deformations, nonlinear response, kinematic 
motion and forces caused by that motion and predict resulting stresses and strains. Details 
can be found in (ALGOR, 2008). 
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*2330 



*11661. 
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Fig. 12. Finite element model of robot manipulators. 
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5.2 Discussion 

The idea here is the creation of multi-axis positioning with consideration of physical 
behaviour through FEM. This was done by manipulating available FE codes constructed for 
boundary conditions. Furthermore, a lot of information can be extracted from the FE 
analysis. The useful information on dynamic structural behaviour includes stress, strain, 
displacements and moment/ torque. The stress-strain and displacement plots, for instance, 
are very important to evaluate the design. The information on torque is useful to choose a 
proper motor size for the real robot. Inertia effect, weight effect and can also be taken into 
account in the model. In this simple example, these factors were ignored. 
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Motions of manipulators were analyzed only for 0.1 s due to computational limitation of FE 
code applied although the motions of the manipulators in reference robot were specified for 
Is in the specification. Even that the computing time took 16 hrs to complete the analysis. 
Fig. 13 demonstrates the positioning of robot manipulators (axes) as defined by boundary 
conditions. It also depicts the deformation of each manipulator due to dynamic loads. 
Fig. 14 - 18 shows the predicted displacements of corresponding manipulators. Table 1 
shows comparison between the rotation of the reference robot manipulators and that of 
virtual robot manipulators. The computational results are consistent with the motion of the 
reference robot manipulators from view point of motion speed. 



Axes 


Ref robot spec. 


Computed 
rotation 


% error 




1 s 


0.18s 


0.18s 




J1 


150° 


27° 


26.5° 


2 


J2 


160 


28.8 


28.87 


0.02 


J3 


170 


30.6 


30.8 


0.06 


J4 


400 


72 


72 





J5 


400 


72 


71.93 






Table 1. Comparison between the actual and computed rotations 




Fig. 13. The deformed plot of robot manipulators. 
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Fig. 14. Computed Rotational displacement of Jl 
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Fig. 15. Computed Rotational displacement of J2 
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Fig. 16. Computed Rotational displacement of J3 
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Fig. 17. Computed Rotational displacement of J4 
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Fig. 18. Computed Rotational displacement of J5 
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Fig. 19. Computed effective stress. 



Fig. 19 shows contour plots of dynamic stress at joints. Contour plots show the deformation 
of each component clearly. The twisting and bending of the manipulator can be clearly seen. 
The main reason is that in current model the friction effect at the contact and clearance were 
ignored. The sizes of holes in mating parts for assembly were the same as if zero clearance. 
Presence of bearing was not there. The size and geometry of manipulators were merely 
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approximation which was not based on proper design calculations. Consequently, severe 
twisting would occur especially at the base where it supports all manipulators and endure 
the dynamically-exerted forces. The computed effective stress is useful to determine the 
failure of each manipulator due to dynamic loading. Fig. 20 shows the computed maximum 
torque required to rotate J5. Computed torque can be used to estimate motor power 
required for robot manipulators. 



Local 2 ■■'.--.: 






Fig. 20. Computed torque. 



6. Conclusion 



The FEM is capable of analyzing multi-axes positioning as well as physical response to 
dynamic load. The current FE code used for dynamic analysis is flexible to analyze physical 
response of the system due to multi-axes positioning involving large displacement. 
Capability of FE code may be different from that of another i.e. FE analysis greatly depends 
on the FE code. Some FE codes have limitations. Such limitations are usually model size (i.e. 
numbers of elements and nodes), meshing, material models, analysis type and solver type. 
In addition, the hardware capacity affects computational power. In future work, weight 
factor, mesh design and an apparopriate material model will be takne into account in the 
finite element modeling and analysis. 
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1. Introduction 

Imitation learning research explores various methods for teaching robots new motions by 
user-friendly means of interaction (Kuniyoshi et al., 1994) (Breazeal & Scassellati, 2002). 
Imitation learning has garnered considerable interest from robotics and artificial intelligence 
communities. 

For robots aimed at household environments, motions such as "to put the dishes in the 
cupboard" are fundamental, but difficult to program beforehand. The reason is that the 
desired motion depends on the size and shape of the dishes, as well as those of the cupboard, 
and also on whether the cupboard has a door. Furthermore, the functional capability of 
natural communication with users is crucial for such assistive robots. However, it is difficult 
to map words or symbols to motions because of the same reason mentioned above. In 
(Kriiger et al., 2007), the difficulties involved in mapping symbols to motions are discussed 
in detail. 

There have been studies which try to solve the problems of mapping symbols to motions in 
the framework of imitation learning. A motion learning/ generation method based on 
hidden Markov models (HMMs) is proposed in (Inamura et al., 2004). (Ogawara et al., 
2002a) and (Ogawara et al., 2002b) present a method in which the relative trajectories 
between two objects are modeled by hidden Markov models (HMMs). Furthermore, we 
have proposed a motion learning and generation method that is based on reference-point- 
dependent HMMs, which enabled the learning of motions such as rotating an object, 
drawing a spiral, and placing a puppet on a box (Sugiura & Iwahashi, 2007) (Haoka & 
Iwahashi, 2000). In (Takano et al., 2007), mocap data is learned by using HMMs, and those 
HMMs are converted for the retrieval of sequential motions. A method based on recurrent 
neural networks is proposed in (Sugita & Tani, 2005). This method is extended to deal with 
sequential motions in (Ogata et al., 2007). 

In this chapter, we present a novel method that generates and recognizes sequential motions 
for object manipulation such as placing an object on another ("place-on") and moving it 
away ("move-away"). In this method, motions are learned using reference-point-dependent 
probabilistic models, which are then transformed and combined. These composite 
probabilistic models are used for the recognition of sequential motions performed by a user. 
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Moreover, motions can be generated from the composite probabilistic models in accordance 
with user instructions, which can then be performed by a robot arm. Fig. 1 shows the 
hardware platform used in this study. The system has multimodal interfaces such as a stereo 
vision camera and a microphone. 

The main advantage of mapping symbols to motions and combing them is as follows. The 
machine can first decompose the given task into learned motions. It can then present a 
planned motion as a sequence of symbols or words. The sequence is grounded on the 
motions taught by the user, and so the user can understand the meaning. This is a 
significant safety feature because if the machine can inform the user of the planned motions 
before executing them, the user can determine in advance whether they are safe or not. 
The rest of this chapter is organized as follows. Section 2 first states the problem we try to 
solve and briefly reviews related work. It then describes the proposed method in Section 3. 
Section 4 shows the results of simulation experiments in which the proposed method 
generated motions by combining learned probabilistic models. The results of physical 
experiments are described in Section 5 in detail. Section 6 discusses problems and possible 
applications with the proposed method. Finally, Section 7 concludes the chapter. 




Fig. 1. Experimental platform used in this study. 



2. Modeling Object-Manipulation 

2.1 Reference-Point-Dependent Motions 

Here, we consider the problem of learning reference-point-dependent motions in the 
framework of imitation learning (Kuniyoshi et al., 1994) (Breazeal & Scassellati, 2002) by a 
robot. 

Clustering manipulation trajectories and mapping them to a verb are not sufficient for verb 
learning if the trajectories are considered only in the camera coordinate system. For 
simplicity, we assume that the mapping between the camera coordinate system and the 
world coordinate system is given, and that the user's utterance is accurately recognized. Let 
us consider the example shown in the left-hand side image of Fig. 2. The figure depicts a 
camera image, in which a user is placing a green puppet on the box. The trajectory itself is 
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meaningless since it depends on the position of the box. In the case of Fig. 2, clustering 
manipulation trajectories in the camera coordinate system works only if the position of the 
does not change. 

On the other hand, if we consider a coordinate system with its center at the box, we are able 
to cluster the trajectories in the coordinate system and map them to a verb. We call such a 
coordinate system as an intrinsic coordinate system. The origin of an intrinsic coordinate 
system is called the reference point. It is to be noted that the origin of the intrinsic coordinate 
system changes with the position of the box. 




Fig. 2. Left: Example shot of camera images. The user is manipulating the green puppet. The 
dotted line represents the trajectory of manipulation. Right: Preprocessed visual features 
obtained from the image stream. 



Regier investigated a model describing the spatial relationship between two objects (Regier, 
1996). He proposed to model verbs as the time evolution of the spatial relationship between 
a trajector and a landmark. Here, a trajector is defined as a participant (object) that is focused 
on. A landmark has a secondary focus and a trajector is characterized with respect to a 
landmark. In cognitive linguistics, words representing spatial relationships such as "away" 
and "left of" are described as the relationship between a trajector and a landmark 
(Langacker, 1987). In (Ogawara et al., 2002b), the relative trajectories between two objects 
were modeled by using probabilistic models. The probabilistic models are used for the 
generation of manipulation trajectories. 

In contrast, the proposed method estimates four components, which are necessary for 
learning object-manipulation verbs, from camera images. The components are as follows: (1) 
the trajector and landmark, (2) the reference point, (3) the intrinsic coordinate system, and 
(4) the parameters of the motion's probabilistic model. Fig. 3 shows examples, the verbs 
"raise" and "move-closer". We can reasonably assume that the reference point of "raise" is 
the trajector's center of gravity. The intrinsic coordinate system can be a Cartesian 
coordinate system as shown in the left-hand figure. In the case of "move-closer", another 
type of intrinsic coordinate system is necessary. In this case, the x-axis of the coordinate 
system passes through the centers of gravity of the trajector and the landmark. As explained 
in Section 4, we assume that there are several types of intrinsic coordinate systems. 
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trajector 

landmark 



reference poiM ■ ^\refeijence point 



; system intrinsic coordinate 






world coordinate system 



Fig. 3. Relationship between trajector/ landmark, a reference point, and an intrinsic 
coordinate system. The spheres, ellipsoids, and box represent objects, and the arrows 
represent the axes of the intrinsic coordinate systems. Left: "raise". The small sphere is the 
trajector, and the reference point is its center. The x-axis of the intrinsic coordinate system is 
horizontal. Right: "move-closer". The direction of the x-axis is toward the trajector from the 
landmark. 

2.2 Motion Learning by Reference-Point-Dependent Probabilistic Models 

Consider that L training samples are given for a verb. Let Vi denote the /th training sample. 
Vi consists of the motion information of the trajector, § , and the candidate set of reference 
points, R/, as follows: 

Vi=(&,R t ), (1) 

£,={v,(f)|r = 0,l,...,T,}, (2) 

y,( f )=[x ! W T ,x,(f) T ,x,(0 T ] T , (3) 

R, ={o ; ,x,(0),x cenler }^{x,. ( \r t =1,2,...,|R ; |} (4) 

where X/(t), x;(t), and X;(f) denote the position, velocity, and acceleration of the trajector, 
respectively; T; denotes the duration of the trajectory; and O; denotes the set of the static 
objects' centers of gravity. The operator | • | represents the size of a set. The reason why O; 

is included in R is that the static objects are candidate landmarks. We also include the first 
position of the trajector, X;(0), in R so that we can describe a motion concept that is 
dependent only on the object's trajectory. Additionally, the center of the camera image, 
"center ' i s added to R to describe motion concepts that are independent of the positions of 
the objects. 

We assume that there are K types of intrinsic coordinate systems, and these types are 
provided by the designer. We denote the type of the intrinsic coordinate system by k. k 
corresponds to a verb, and the reference point corresponds to each V/. We obtain the 
estimated intrinsic coordinate system for the Ith data from the estimation of k and the 
reference point x r . 

Let ' J Yj denote the trajectory in the intrinsic coordinate system Cj, (x,. I. Henceforth, 

parameters in a particular coordinate system are written in a similar manner. Now, the 
index series of reference points, r = {)"/ 1 1 = 1, 2, ..., L} , the type of the intrinsic coordinate 
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system, k, and the parameters of a probabilistic model regarding trajectories, X , are 
searched for using the following maximum likelihood criterion: 

L 



(r, k, Xj = argmax V log P{Y, \r,,k, X) (5) 

t,k,h i -. 
L 

= argmax Y log p( Q (x '< ) $ l ; X) 



where • represents estimation. In (Sugiura & Iwahashi, 2007) and (Haoka & Iwahashi, 2000), 
the solution to Equation (5) is explained in detail. 

3. Combination of Reference-Point-Dependent HMMs 

3.1 Transformation of HMMs 

Here, we consider the problem of the recognition and generation of sequential motions 

based on composite reference-point-dependent HMMs. 

In speech recognition, HMMs are transformed for speaker adaptation by using 

transformation matrices. Here, the transformation matrices are independent of the order of 

HMMs. However, we cannot combine two reference-point-dependent HMMs in this manner. 

This is because the jth HMM parameters are dependent on the (/' - l)th HMM parameters 

(Fig. 4). 

Fig. 5 illustrates an example of the process of combining two reference-point-dependent 

HMMs. To combine HMMs corresponding to "raise" and "move-closer," the output 

probability distributions of each HMM must be transformed since they represent the 

distributions on different coordinate systems. 

An advantage of transforming intrinsic coordinate systems is the smoothness of the 

composite trajectories. In the proposed method, velocity and acceleration data as well as 

position data are used for learning. For safety reasons, changes in the velocity and 

acceleration data should be continuous. It is therefore important to obtain smooth 

trajectories of X;(f) and X;(f) when combining two HMMs. Let us consider a case in which 

verbs dependent only on velocity information, e.g., "throw," are to be combined. If two 

HMMs were simply aligned to generate the composite trajectory, the velocity changes might 

be discontinuous in this case. In contrast, the proposed method, which is described in detail 

below, generates a smooth trajectory. 

Now we consider the problem of obtaining a composite HMM from the transformation and 

combination of reference-point-dependent HMMs. Let X~ ! ' and C' ; ' denote the parameters 
and the intrinsic coordinate system, respectively, of the jth HMM. Those HMMs are 
modeled as left-to-right HMMs. The output probability density function of each state is 

modeled by a single Gaussian. The mean position vector at state s, L |i x (s) , is transformed 
by the following homogeneous transformation matrix: 

V,(s)l 

1 



1 



c« 








(j=l,2,...,D,s = l,2,...,S j ), (6) 
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where ,,sR denotes the rotation matrix from C'-'' to the world coordinate system W. 
Furthermore, s = and s = Sj+1 are defined as the initial and final states of the jth HMM, 
respectively. The mean vector of velocity, V-i( s ) > and the mean vector of acceleration, 



-(/> 



jij:(s) , are rotated as follows: 



v,( S )= c( ^ c< V( s ) 



(7) 
(8) 



In contrast, the diagonal items of covariance matrices for position are approximated as 
follows: 



diag w Z ;c ( S ) = diag cW i: x ( S ) 



(9) 



where 2 T (s) and S x (s) denote the covariance matrices at state s in coordinate systems 

W and C' ; ' , respectively. The non-diagonal items of the matrices are equal to zero. The 
matrices for velocity and acceleration are transformed by the same simple approximation. 
We do not perform a rotation of the covariance matrix because the HMM-based trajectory 
generation method (Tokuda et al., 1995) that we use does not deal with full covariance 
matrices. 
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Fig. 4. Schematic of the combination of two reference-point-dependent HMMs. W 
represents the world coordinate system. 
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Fig. 5. Example of transformation in the combination of two HMMs, "raise" and "move- 
closer". Each dotted circle represents the variation of output probability distributions at 
each state of a left-to-right HMM. The direction of state transition is indicated by the color 
darkness. The intrinsic coordinate system of "move-closer" is transformed so that the its x- 
axis passes through both the landmark (the reference point of "move-closer") and the last 
position of the HMM regarding "raise". The dotted line represents the composite trajectory. 

3.2 Generation of Motion Sequences by Composite HMMs 

Here, we consider the problem of generating trajectories of sequential motions from 
composite HMMs. Suppose that a static image and the index of the trajector are given. As 
given in Section 2.2, we extract the candidate set of reference points, R. Our proposed 
method deals with two types of motion generation: (1) explicit instruction and (2) target 
instruction. 



3.2.1 Explicit Instruction 

The user requests the robot to move an object according to his/her instruction, which 
consists of a sequence of motions. Inputs from the user are the object ID and a sequence of 
verb-landmark pairs. The proposed method outputs the maximum likelihood trajectory that 
accomplishes the sequence. 

Suppose a set of verbs, the intrinsic coordinate systems corresponding to verbs, and the 
HMM parameters corresponding to the verbs are given. Let V = [v t | i = 1,2,..., |V|j denote a 
set of verbs, A t denote HMM parameters corresponding to verb u„ and k, denote the index 
of intrinsic coordinate system corresponding to verb Vi. Let (i,r) denote a D-tuple of verb- 
landmark pairs as follows: 



(Lr)^,^) i< D >,r<V <2 > r< D >) 



(10) 



The candidate set of reference points, R, can be obtained from an image stream (c.f. Section 
2.2). The method explained in Section 3.1 provides a composite HMM A D (i,r). 
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The trajectory of Object r tra - corresponding to the verb-landmark pairs, (i, r), is obtained as 
follows: 

| = argmax pfe | r^,Q D (i),r,R) 



= argmax pfe | x traj ,Q D (i), A D (i,r)) (U) 



where Qd(i) denotes the state sequence of the HMM corresponding to verb »,, and x tra j 

denotes the initial position of the trajector. The method explained in (Tokuda et al., 1995) 
provides the maximum likelihood trajectory from the unknown state sequence Qd(i)- 

3.2.2 Target Instruction 

Next, we consider the problem of obtaining the optimal index sequence of verb-landmark 

pairs, li,r ), which affords the maximum likelihood trajectory £ from initial position x tra j to 
the goal position x G . Most motion planning methods (see e.g., (Latombe, 1991)) do not 

provide linguistic expressions that explain generated trajectories. In contrast, the proposed 
method can generate trajectories consisting of learned motions labeled by verb indices. 
Therefore, the proposed method allows a robot to explain generated trajectories by natural 
language expressions if it has a speech interface. For example, the robot can generate 
confirmation utterances such as "The robot will put Object A on Object B, then raise Object 
A, is it OK?" Generating such confirmation utterances is desirable from the viewpoint of 
safety since the user can judge whether the motion is appropriate or not before the planned 
motion is performed. 

In Target Instruction mode, the user requests that the robot move an object to a goal. 
Inputs from the user are the object ID and goal position x G . The proposed method outputs 

the maximum likelihood sequence of verb-landmark pairs, |i,f), and the maximum 

likelihood trajectory £, . We obtain \£,i,i I by conditioning the right side of Equation (11) 
with x c and then adding (i,r) to the search arguments: 

(|,i,r)=argmaxP(^|x lIa j,x G ,Q D (i) / A D (i,r)), 

(AaJD 
where the number of combined HMMs, D, is a search depth parameter. 

3.3 Recognition of Motion Sequences by Composite HMMs 

The recognition of sequential motions by reference-point-dependent HMMs can be 
formalized as the problem for obtaining the maximum likelihood probabilistic model for 
trajectory £ under the condition where a lexicon of verbs L v =\v i ,X i ,k i \ i =l,2,,„,\V\] is 

given. The maximum likelihood index sequence of the verb-landmark pairs, li,f), is 
searched through the following equation: 
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(i,r)= argmaxP(£ | i,r,D,R) 

i,r,D 

= argmaxP(£|A D (i,r)) 



4. Simulation Experiments 

4.1 Experimental Setup 

We first conducted simulation experiments for evaluating the proposed method. The 

simulator consists of a graphical interface and a mouse. Virtual objects shown on the screen 

can be manipulated by dragging them with a mouse. 

In the learning phase, a user was asked to teach motions. The trajectories were recorded and 

used for training probabilistic models. The trajectories for the following seven verbs were 

collected. 

raise, move-closer, move-away, rotate, place-on, put-down, jump-over 

For each verb, the number of training samples was 15. Those motions were taught by the 
user in the learning phase beforehand, and they were constant throughout the motion 
generation experiments. 

The verbs were successfully learned in the experiment. Fig. 6 shows the examples of the 
training samples. In this figure, the thick arrows represent the trajectories of an object 
manipulated by the user. The thin arrows represent the x- and y-axes of the estimated type 
of the intrinsic coordinate system. The type name is shown at the lower right of each 
illustration. The types of the intrinsic coordinate system are defined as follows: 

Cf. A coordinate system with its origin at the landmark position. Cj is a translated 

camera coordinate system. The x-axis is inverted in case the x-coordinate of the 

original position of the trajector is negative after translation. 
Ci An orthogonal coordinate system with its origin at the landmark position. The 

direction of the x-axis is from the landmark towards the trajector. 
C3: A translated camera coordinate system with its origin at the original position of the 

trajector. 
C4: A translated camera coordinate system with its origin at the center of the image. 

After probabilistic models were trained, we carried out motion generation simulation 
experiments. Two types of motion experiments were performed: for target instruction and 
for explicit instruction. 

In the target instruction experiment, the user requested the robot to move an object in a 
camera image. The inputs were object ID and a goal point, and the outputs from the robot 
were both a sequence of verb-landmark pairs and a trajectory to the goal. We used 64 grid 
points as goal points. The search depth parameter D was set as D = 3. Therefore, the 
estimated motion sequence consisted of up to three HMMs. 
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Fig. 6 Examples of training samples. 



Statistical Imitation Learning in Sequential Object Manipulation Tasks 



599 



4.2 Result (1): Motion Generation 
4.2.1 Explicit Instruction 

The explicit instruction experiment was carried out in the same simulation environment. 
Fig. 7 shows two examples of motion generation: "jump object 1 over object 2, then put 
object 1 down, and move object 1 closer to object 4" and "jump object 2 over object 1, jump 
object 2 over object 1 again, and then place object 2 on object 5". The inputs for the two cases 
were as follows: 





Trajector 


Motion sequence 


(a) 


Object 1 


<jump-over, 2><put-down, no landmark><move-closer, 4> 


(b) 


Object 2 


<jump-over, lxjump-over, 1> <place-on, 4> 



Fig. 7 shows that the three motions were combined smoothly. To examine this result 
quantitatively, we plotted the evolution of position, velocity, and acceleration in case (a) in 
Fig. 8. Fig. 8 verifies that the composite trajectory of velocity and that of acceleration were 
continuous. 
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4.2.2 Target Instruction 

The simulation environment is shown in Fig. 9. There were five objects in the environment 
(depicted by numbered boxes and circles). Object 1 was used as the trajector. Fig. 9 shows 
the examples of maximum likelihood trajectories output by the proposed method. In the 
figure, bracketed pairs represent the estimated sequences of verb-landmark pairs. For 
example, <place-on, 2><move-closer, 5> signifies "place object 1 on object 2, and move 
object 1 closer to object 5." 

From Fig. 9, we can see that the proposed method combined two motions smoothly rather 
than independently. In particular, although <move-away, 2> and <move away, 2><jump- 
over, 4> share <move-away, 2>, the trajectories do not overlap with each other. This is 
probably due to the large variation for the last position in the learned probabilistic model of 
"move-away." In other words, a part of the trajectory of <move-away, 2> was curved to 
smoothly combine <move away, 2> and <jump-over, 4>, but the likelihood of the resultant 
trajectory was still high because of its large variance. 
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Fig. 9. Generated motions in Target Instruction mode for manipulating object 1. Numbered 
boxes and circles represent objects. Bracketed pairs represent the estimated sequence of 
verb-landmark pairs. Specifically, <place-on, 2><move-closer, 5> means "place object 1 on 
object 2, and move object 1 closer to object 5". 



5. Physical Experiments 

5.1 Experimental Setup 

The experiments were conducted by using a PA-10 manipulator manufactured by 
Mitsubishi Heavy Industries with seven degrees of freedom (DOFs). The manipulator was 
equipped with a BarrettHand, a four-DOF multifingered grasper. The user's movements 
were recorded by a Bumblebee 2 stereo vision camera at a rate of 30 [frame/ s]. The size of 
each camera image was 320 x 240 pixels. The left-hand side image of Fig. 2 shows an 
example shot of an image stream, and the right-hand side image of the figure shows the 
internal representation of the image stream. All the motion data used for learning and 
recognition were obtained from physical devices. In addition, motion generation results 
were examined in an environment using the manipulator and physical objects such as 
puppets and toys. 
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The difference of parameter setup between the simulation and physical experiments was the 
number of training samples, L. In physical experiments, L was set to 9 for each motion. 

5.2 Motion Generation 

5.2.1 Explicit Instruction 

Fig. 10 shows an example trajectory generated by the proposed method. The solid line 

represents the trajectory generated in the explicit instruction mode. The input for the explicit 

instruction mode was as follows: 



Trajector 



Motion sequence 



Object 2 



<move-away, lxjump-over, 4><move-closer, 4> 



From Fig. 10, we can see that the proposed method generated an appropriate trajectory. To 
support this, the manipulator is shown performing the generated trajectory, as shown in 
Fig. 11. 




Fig. 10. Generated trajectory in the explicit instruction mode. 




Fig. 11. Sequential photographs of the manipulator executing the trajectory shown in Fig. 10. 
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5.2.2 Target Instruction 

For the target instruction mode, the top three trajectories are shown in Fig. 12. The trajector 
ID was set to 1, and the goal position used is indicated in the figure. In the figure, the solid, 
broken, and dotted lines represent the best, second-best, and third-best trajectories, 
respectively. In addition, the top three verb-landmark pairs and the log likelihood are 
shown in the figure. 




1. (solid line) <jump-over, 3> <move-closer, 2> -16.45 

2. (broken line) <jump-over, 3> -18.66 

3. (dotted line) <place-on, 3> <move-closer, 2> -25.06 
Fig. 12. Generated trajectories in the target instruction mode. 



5.3 Motion Recognition 

The user was presented with six pairs of randomly chosen verbs, and performed the 
motions sequentially. The manipulation trajectories and the positions of the static objects 
were recorded to obtain a test set. For each pair, five different object settings were given. 
Therefore, the size of the test set was 30. 

Fig. 13 illustrates example trajectories in the test set. In the figures, the top three recognition 
results for each scene are shown. The bracketed pairs and numbers represent the estimated 
sequences of verb-landmark pairs and the log likelihood, respectively. 

We can see that a correct recognition result was obtained for the left-hand figure of Fig. 13. 
On the other hand, the correct recognition result for the right-hand figure does not have the 
maximum likelihood. This is considered to be due to the fact that the trajectory in the 
training set always starts from pause (x,(0) = 0), and therefore, the composite HMMs 
contain states representing pauses between motions. However, the two motions are 
consecutively performed. Therefore, the likelihood of such trajectory given the combined 
HMMs was smaller than the likelihood of the trajectory given an HMM. 

Table 1 shows the number of correctly recognized samples. The column labeled "w-best" 
stands for the number of correct answers contained in the top n recognition results. The 
accuracy of 1-best, 2-best, and 3-best recognition results are 63%, 83%, and 87%, respectively. 
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In the table, we obtain an accuracy of 80% (12/15) for sequences (1), (3), and (4). This is 
reasonable since we have obtained an accuracy of 90% for the recognition of single motions 
in preliminary experiments. However, we obtain an accuracy of 47% (7/15) for sequences 
(2), (5), and (6), which contains at least one Ci verb. This result also supports the fact that the 
approximation (Equation (9)) deteriorated the recognition accuracy. 




1. <place-on, 3> <place-on, 2> : -22.04 1. <move-away, 2> : -22.18 

2. <jump-over, 2> <place-on, 2> : -23.79 2. <rotate> <move-away, 2> : -22.65 

3. <place-on, 3> <move-away, 3> : -28.79 3. <rotate> : -25.06 

Fig. 13. Examples of test set. The recognition results for each scene are shown below the 
corresponding figure. Left: "place Object 1 on Object 3, then place Object 1 on Object 2." 
Right: "rotate Object 1, and then move Object 1 away from Object 2." 



Test set 


1-best 


2-best 


3-best 


(1) rotate + rotate 


5 


5 


5 


(2) move-away + move-closer 


3 


3 


3 


(3) place-on + place-on 


3 


5 


5 


(4) rotate + jump-over 


4 


4 


4 


(5) rotate + move-away 


2 


4 


4 


(6) move-closer + place-on 


2 


4 


5 


Total 


19/30 
(63%) 


25/30 
(83%) 


26/30 
(87%) 



Table 1. Number of correctly recognized samples. 



5. Discussion 

5.1 Recognition Accuracy 

Here, we discuss two causes for the deterioration in the recognition accuracy. 

The first one is that sequential motions performed by users tend to be smoothly combined. 

However, the likelihood for such motions is not always high. This is because the trajectory 
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in the training set always starts from pause (x,(0) = ), and therefore, the composite HMMs 

contain states representing pauses between motions. 

We assume that this problem can be solved by using pause HMMs. In this case, a sequence 

of HMMs comprising a motion HMM sandwiched between pause HMMs are trained. 

Furthermore, we can obtain a composite HMM by aligning the HMMs of pause, motion A, 

motion B, and pause, and thereby combining two motions. 

Another problem is that Equation (9) does not consider the full covariance matrices, and so 

the rotation of coordinate systems is ignored. As stated above, this approximation 

deteriorated the recognition accuracy for the C2 verbs. In the future work, we will perform 

the rotation of covariance matrices. 



5.2 Collision Avoidance 

The proposed method has a possibility of generating inappropriate trajectories leading to 
object collision. For instance, Fig. 9 shows that the trajectory of <move-closer, 2><move- 
closer, 3> runs through Object 2. 

There are at least three solutions to this problem. The first is to select the maximum 
likelihood sequence of verb-landmark pairs among which no collision occurs. This is the 
simplest solution since the positions of all static objects are evident in camera images. 
Another solution is to slightly change the maximum trajectory so as to avoid collisions. The 
third one is to modify the output probability density functions of HMMs by setting them to 
near the position of obstacles. The third method, however, will not work if there are many 
obstacles in the camera image. 

5.3 Future Work 

The proposed method can be applied for motion planning rather than manipulating objects. 
One example is a mobile robot that generates a path to a goal set by the user by combining 
learned motions and informs the user about it. Suppose a camera is placed on the ceiling of 
an office and a camera image, as shown in Fig. 9, is obtained. In this case, the robot can 
decompose an instruction such as "go to the president's room" into learned motions such as 
"move-forward" and "move-along." 

Another application would be the prediction of motions. In this chapter, partial trajectories 
were not considered. However, if the motion recognition is performed with a part of the 
trajectory, the system can predict the landmark of the motion and help the user. For example, 
this could be used in a technique to unlock a door before the user grasps the door knob. 

6. Conclusion 

It is important for robots to be able to report their internal states to humans in a 
comprehensive manner in environments shared by both. For example, it is critical for the 
safety of people that the robots are able to communicate their next move. 
In this chapter, we have presented a method to combine reference-point-dependent 
probabilistic models. The experimental results of the Target Instruction mode revealed that 
the proposed method successfully decomposed goal-oriented motions into learned motions. 
This indicates that the robot could decompose the given task into learned motions and then 
present the planned motions, in a manner easy for the user to understand. This is a 
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significant safety feature because if the machine can inform the user of the planned motions 
before executing them, the user can decide in advance whether they are safe or not. 
Furthermore, the proposed method enables the recognition of sequential motions. One of 
the contributions of this work is the recognition of sequential object manipulation. In the 
future work, the proposed method would be applied to problems for the retrieval of 
motions from video data and action mining in ubiquitous computing. 
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1. Introduction 

Various modes of tangible interfaces have been explored and researched. In this chapter we 
limit our look at tangible user interfaces to a subset of these. The subset is characterised by 
portability and no attached tethers, be they mechanical links or electrical wires. The subset 
does include tangible objects that are connected to a larger system for the purpose of relative 
position and orientation detection, if relevant. Such detection mechanisms include optical, 
magnetic, and radio means. Examples of optical detection are the use of fibre optics and a 
video camera. Magnetic detection utilises either the presence of a magnetic field, or the 
changes in such a field. Radio detection mechanisms include the use of the Global 
Positioning System (GPS) and radio frequency identification (RFID). Using electrically 
conductive pins provides for another untethered system. 

Electrical field sensing and the use of acoustic waves are also covered in this chapter. 
In our discussion we assume open-loop control of robot manipulators, that is, the user 
interface does not receive feedback from sensing subsystems. The user interface relies on 
other subsystems to check the inputs provided by the user interface with the actual position 
of the manipulator. 

2. A Short Introduction to Tangible User Interfaces 

It this section we introduce the novice to this exiting mode of interfacing to technologies. We 
look at the properties of known Tangible User Interfaces (TUI's) and how they have been 
applied in the real-world. 

What are Tangible User Interfaces (TUI's)? The term TUI has been coined by Ullmer and 
Ishii in 1997 (Ullmer, 1997). This definition is somewhat restrictive in that the output is also 
reflected in the input device. An example of such a device is Tobopo. Tobogo is a physical 
device that will record the actions the user has taken on its various components. For 
example, if the user constructs a model dog and moves the various legs, the system will 
record the motions and replay them. It is quite possible to let the system modify the 
behaviour after being recorded, or show a response even during the recording. 
In this chapter we look at a broader definition of TUI, similar to the relaxation of TUI's by 
others (Fisken, 2004). In the definition we address cubic objects that provide an input to 
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some system. The output is not manifested in the cubes as per the strict definition of TUI's. 
As applied to robot effectors, this implies that the output is visible through the change in the 
effectors' state. For the purpose of this chapter we prefer the broad script of TUI's as given 
in Fishkin2004. In this broad script we are concerned with an "input event", some system 
that "senses" the event and somehow responds to it, and some form of feedback initiated by 
the system which is called an "output event". 

We base our interaction roles on those described elsewhere (Yanco, 2004). In the taxonomy 
of Yanco, five interaction roles are given. These are "a supervisory role", "an operator", "a 
teammate", "a mechanic", and "a bystander". The tangible interfaces we describe are best 
suited in the role of a supervisor. The supervisor constructs the series of actions that should 
be executed by the robot and then activates the programme represented by the cubes. The 
underlying system does not simply execute a number of steps, but has the ability to change 
the execution sequence based on inputs received from the actuators, the environment, or 
another system (Fig. 1.). 

Some three dimensional TUI's are manipulated in a two-dimensional plane. Other TUI's 
have been developed that also work in three dimensional spaces, such as Tobopo, 
ActiveCubes, and SystemBlocks. Tobopo can be used as an autonomous system. 
ActiveCubes are used to sense and interface with other systems. 
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Fig. 1. Generic tangible system diagram 



3. Why TUI and not GUI? 

Ever since the electronic computer became a research tool the operator had to take care of 
the delicate input mechanisms available to interact with the computer. At first the 
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mechanisms available were switches and paper tape. These were followed by magnetic tape 
and paper punch cards. At this time output mechanisms evolved from paper tape and 
lights, to the two-dimensional cathode ray tube (CRT) display. Yet these output mechanisms 
are still two-dimensional. The information displayed on these displays has progressed from 
only textual to the incorporation of graphical elements. Over time the Graphical User 
Interface (GUI) has become familiar to all computer users. Yet some users still insist that the 
textual interface suits them the best. They claim that they are the most productive with such 
an interface. At the same time these users also make extensive use of the QWERTY keyboard 
to interact with the computer. They are professional computer system developers and make 
little use of the computer mouse, claiming that the keyboard shortcuts they are accustomed 
to empowers them more than using a mouse and the GUI. For the majority of computer 
users the GUI and mouse remains the most prominent interface to the electronic computer. 
There exists, however, a relatively new research field in which the manipulation of physical 
artefacts are considered as an alternative interface to the electronic computer. It can be 
argued that making use of tangible interaction with the computer, in addition to the GUI, 
increases the 'bandwidth' available to a user for interacting with the computer. An increased 
bandwidth allows for faster interaction. The use of gross motor skills, in addition to the fine 
motor skills required for operating a computer mouse, might be more 'natural' for some 
users. 




Fig. 2. Graphical User Interfaces address the user's cognitive skills. Tangible User Interfaces 
also incorporates the user's motor skills (Hegeveld 2009) 
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Fig. 3. TUI instantiations of GUI elements (Ullmer 1997) 
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In the physical world in which we use Tangible User Interfaces (TUI's), we can find 
similarities between the TUI's and the GUI's by extrapolating the two dimensional screen to 
the three dimensional physical world. 

4. Limitations and Advantages of TUI's 

TUI's have certain advantages and limitations compared to other technology interfaces. 
These advantaged and limitations are discussed in this section. 

4.1 Limitations 

Tangible User Interfaces have a number of disadvantages over conventional Graphical User 
Interfaces. These include storage of the constructed sequence, the space required for the 
sequence, how to make it persistent (as one would save a file to a hard disk), how to 
document it and transporting the constructed sequence. 




Fig. 4. Transporting TUI sequences can be difficult (Horn 2009) 



4.2 Advantages 

Tangible User Interfaces can potentially be designed to be intuitive for the novice user 
(Fig. 5.), but potentially frustrating for an advanced user. A textual interface or an iconic 
interface could be presented to the advanced user as a possible solution. 




Fig. 5. Tangicons (Scharf et al., 2008) 
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5. Data Coupling Mechanisms 

5.1 Magnetic 

Magnetic detection is possible using either mechanical switches or solid state detection 
circuitry. Figures 6 and 7 illustrate a system called GameBlocks which makes use of 
mechanical "reed" switches. 




Fig. 6. GameBlocks (Smith 2007) 



Fig. 7. GameBlocks (Smith 2009) 



5.2 Electrical contact 

Electrical contacts rely on direct physical contact between two or more electrically 
conductive components. A few examples follow. 

AlgoBlocks 

AlgoBlocks makes use of wide electrical connectors to distribute the data through the 
system. 




Fig. 8. AlgoBlocks (Suzuki 1995) 



FlowBlocks 

FlowBlocks distributes data using the same magnets that are used to keep the various 
components together. 
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Fig. 9. Flowblocks are connected using magnets. The same magnets are also used to transfer 
data and power between the blocks. The insert shows three magnets at the end of one of the 
blocks. Magnets assist is aligning the blocks properly (Zuckerman 2005) 

VIO controls 

VIO controls make use of pins which consist of two parts each. One part runs along the 
inside of the other and is slightly longer than the outer sleeve. The longer length allows 
penetration to a second conductive layer which is located below the upper conductive layer. 
The sleeve makes contact with the upper layer only. 




Fig. 10. VIO controls (Villar and Gellersen) 
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Fig. 11. The electrical configuration of VIO controls. (Adapted from Villar and Gellersen) 
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Fig. 12. An example of the VIO controls being applied 



5.3 Optical: video camera from below 

This approach makes use of bottom projection (Fig. 13.) with the video camera placed below 
the work surface. A configuration like this is convenient as it eliminates obscuration of both 
the projection and video recordings (Fig. 14.). 

5.4 Optical: video camera from above 

In the previous section an example in given of fiducial markers (Fig. 15.) placed at the 
bottom of the object being tracked. Another configuration is with the fiducial markers 
placed on top of the object to be tracked (Fig. 16.). Optional images are also projected from 
above the interaction surface. 
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Fig. 13. Using tangibles tagged with fiducials to control and actuator. Visual feedback is 
provided by the projection below the transparent work surface (Adapted from 
Kaltenbrunner and Bencina) 
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Fig. 14. Tangibles with fiducials and bottom projection are used to control a music 
synthesiser in a system called "reacTable" 




Fig. 15. Examples of fiducial marker types. The fiducial on the left is very compact. (Adapted 
from Kaltenbrunner and Bencina) 



When using Illuminating Light (Fig. 17.), a software programme identifies the coloured dots 
and their patterns on the optical elements. A projector then adds additional information 
onto the work surface, such as the path of reflected light. 

Tern (Fig. 18.) consists of a collection of interlocking pieces. Each piece has a unique optical 
pattern imprinted on the top which identifies the function of that piece. 




Fig. 16. Top camera and top projection (Kirton 2008) 
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Fig. 17. Illuminating Light (Underkoffler 1999) 




Fig. 18. This tangible interface consists of wooden blocks shaped like jigsaw puzzle pieces 
(Horn2009) 



5.5 Optical: one dimensional 

In contrast to the two-dimensional video camera communication mechanism described 
earlier in this chapter, we here present two examples of TUI systems that make use of a 
single light source that communicates between two blocks (Fig. 19, 20.). 
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Fig. 19. Computational alphabet block (Eisenberg 2002) 




Fig. 20. Using the Navigation Blocks to construct disjunctions and negations. Left: "or' 
query. Middle: "and" query. Right: "not" query (Camarata 2002) 



5.6 Acoustic sensing 

The acoustic table (Fig. 21.) consists of a number of acoustic transmitters which are used to 
'illuminate' the surface of the interaction area. The objects to be detected contain circuitry 
that responds to the "illumination" by transmitting infrared signals to a set of infrared 
detectors around the table. 



5.7 Induction sensing 

Induction sensing systems (Fig. 22.) make use of low frequency alternating current flowing 
through a wire grid. The objects to be sensed contain their own inductive and capacitive 
circuits which resonates at a pre-determined frequency. If the sensing surface is stimulated 
at the same frequency at which the object to be sensed has been tuned, the object will be 
detected. 
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Fig. 21. Acoustic table (Mazalek) 




Fig. 22. Several modified sensing antennas and LC tag (Patten 2005) 
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Fig. 23. Resonant table in use with overhead projection (Patten 2005) 
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6. Other TUI's 

We have not covered the multitude of possible sensing and construction mechanisms. In this 
section we simply provide a few more interesting examples. 




Fig. 24. Grid-restricted tangibles (Frazer 1995) 



The following TUI's are not restricted to a surface for assembly. They operate independently 
of a surface and can be manipulated in the hand of the user while in operation. 

6.1 Tobopo 

Topobo (Fig. 25.) consists of a number of building elements, most of which contain electrical 

circuits. 

Some elements serve as sensors, others as actuators. What makes Tobopo unique is that 

some building elements contain both a sensing circuitry and actuators. As an example, if the 

user rotates the shaft of a motor element, the Tobopo system can record that action and on 

command 'replay' the action. 




Fig. 25. Tobopo programming and replay (Raffle 2008) 
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6.2 SystemBlocks 

SystemsBlocks (Fig. 26.) consists of a number of objects with embedded electronic circuitry. 
These augmented objects are interconnected using electrical wires through which data 
flows. 




Fig. 26. SystemBlocks are interconnected using electrical wires (Zuckerman 2004) 



6.3 ActiveCube 

ActiveCube (Fig. 27.) is a system comprising of various cubes, each containing an electric 
circuit specific to the cube's intended function. The cubes are custom designed to serve as 
either sensors or actuators. Examples of sensor cubes are a sound processor, an infrared 
sensor, a gyroscopic sensor, a tactile sensor, and an ultrasonic sensor. Examples of actuator 
cubes are a motor, a buzzer, a vibrator, and a light. Activecubes are snapped together using 
the four clothing fasteners on each of the six cube surfaces. These fasteners are also used for 
transferring data between the cubes. 
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Fig. 27. ActiveCube (WATANABE 2004) 
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7. Tangible Interfaces Research at the CSIR's Meraka Institute 

In the research described in the above sections, little or no consideration has been given to 
the costs involved in creating the tangible interfaces. A different approach is followed at the 
CSIR's Meraka Institute. 

The Institute is located in the developing region of Southern Africa where access to funding 
is limited, perhaps more so than in developed regions where the research covered above is 
taking place. As a result the cost of technology is considered a very important system 
component. To achieve the objective of affordable Tangible Interfaces for developing 
regions, the Institute explores various materials and technologies. In all its research to date, 
the Institute has made use of low cost electronic components for interfacing the tangible 
objects to toy robotic devices (Smith, 2008). 

The approach followed at the Institute, which distinguishes it from the others mentioned, is 
that of leveraging communal knowledge and the use of low-cost technologies. To this end, 
one of the research objectives is to develop a modular system in which various community 
members can collaborate in the co-creation of a robotic system using tangible interfaces. 
When realised, one team member will assemble a simple, low cost electronic circuit. In turn, 
another team member will design and craft Tangible Interface objects. The electronic circuit 
and the crafted object will then be integrated to form a Tangible Interface. This Tangible 
Interface can then be manipulated by the end user. The purpose of the electronic circuit is to 
sense the position and orientation of the tangible object and then send commands to a robot. 
Examples of robots used in the research include humanoid robots and LEGO cars (Fig. 28.). 

7.1 Technology 

The sensing mechanism is common to all the prototypes described in this section. In these 
prototypes the sensing of a Tangible Interface object is accomplished through a combination 
of low-cost reed switches and permanent magnets. A number of reed switches are mounted 
on a sensing platform and magnets are embedded inside Tangible Interface objects. When a 
Tangible Interface object is placed on top of the sensing surface, a pre-determined 
combination of reed switches close. At the same time an electronic circuit senses the state of 
the reed switches and sends appropriate instructions to a robot for execution. 

7.2 Prototypes 

Cubic - and rotational Tangible Interfaces prototypes are described in the following sections. 

Cubic Interfaces 

Initial research at the Meraka Institute made use of acrylic sheets. These were cut according 
to a profile which allows assembly into a cube without the need for adhesives (Fig. 29.). Low 
power laser facilities at a local FabLab (Gershenfeld, 2005) were of immense value in 
completing this task (Smith, 2006). As a side it can be noted that FabLab is a concept which 
originated at the MIT Media Lab with the objective of making advanced prototyping 
technologies available to communities in developing regions. 

The second prototype was constructed from commercially available closed-cell foam 
squares. These squares are manufactured in large quantities for use in baby and toddler 
rooms (Fig. 30.). The bright colours and soft texture afforded by these foam squares are ideal 
for young users of the Tangible Interfaces (Smith 2009a). 
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Both the acrylic- and foam cube- designs make use of a sensor matrix to detect the tangible 
object. This cubic configuration has been dubbed "GameBlocks". 




Fig. 28. Two toy robots used in the Tangible Interfaces research 




Fig. 29. The acrylic GameBlocks consists of cubes (left) and sensing trays (center) 




Fig. 30. Closed-cell foam GameBlocks 



Rotational interfaces 

A different sensor configuration was tested in the third and fourth prototype designs. In this 
configuration all tangible objects are identical in both shape and function, the difference 
being the spatial orientation of the tangible being manipulated. By changing the 
configuration of sensors inside the sensing surface as well as that of the embedded magnets 
inside the tangible, a configuration for sensing rotation was realised. 
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In the third prototype the properties of soft rock was explored. Using hand tools, the end 
user can easily shape the soft rock to create a personalized tangible (Fig. 31.). This prototype 
has been dubbed "RockBlocks" (Smith, 2009b). 

"Dialando" is, similar to RockBlocks, a tangible interface based on rotational information. 
This fourth prototype demonstrates the use of recycled materials in its construction. A 
tangible object is constructed by sandwiching low cost magnets between two discarded 
CD/DVDs and finishing the construction off with a section of discarded electrical cord. 




Fig. 31. RockBlocks and Dialando 

7.3 Problems and solutions 

A common problem experienced in various degrees is that of aligning the tangible object 
with the sensing surface. If the alignment is slightly out, the tangible object will either not be 
sensed or will be sensed incorrectly. The fourth prototype described above is an attempt to 
address this problem. 

In an effort to reduce alignment problems a neodymium magnet was incorporated at the 
centre of the tangible object (Fig. 31.). A matching magnet was also positioned in the center 
of the sensing surface. Being of opposite polarity, the two magnets pull the tangible object 
into place when approaching the sensing surface, thus eliminating most of the misalignment 
problems experienced in the other designs. 

7.4 Future work 

In the design of the Dialando prototype, most of the alignment problems have been 
addressed through the addition of magnet-pairs. What still needs addressing is how to limit 
rotation of the tangible object to discrete angles. It is anticipated that this can be 
accomplished using a similar mechanism as that implemented for solving the misalignment 
between the sensing surface and the tangible object. 
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1. Introduction 

In recent years, establishing more and more explicit, complete and accurate dynamic models 

for the special category of flexible link manipulators has been a formidable challenging and 

still open problem in robotics research. 

This chapter is devoted to a methodological presentation of the application of Timoshenko 

beam (TB) theory (TBT) concepts to the mathematical description of flexible link robotic 

manipulators dynamics, as a more refined modeling approach compared to the classical 

Euler-Bernoulli (EB) theory (EBT) which is the conventionally adopted one. 

Compared with the conventional heavy and bulky rigid robots, the flexible link 

manipulators have their special potential advantages of larger work volume, higher 

operation speed, greater payload-to-manipulator weight ratio, lower energy consumption, 

better manoeuvrability and better transportability. However, their utilization incurs a 

penalty due to elastic deformation and vibration typically associated with the structural 

flexibility. As a consequence, the motion planning and dynamics modeling of this class of 

robotic manipulators are apparently made extremely complicated, as well as their tip 

position control. 

The complexity of modeling and control of lightweight flexible manipulators is widely 

reported in the literature. Detailed discussions can be found in (Kanoh et al., 1986; Baruh & 

Taikonda, 1989; Book, 1990; Yurkovich, 1992; Book, 1993; Junkins & Kim, 1993; Canudas de 

Wit et al., 1996; Moallem et al, 2000; Benosman et al., 2002; Robinett et al., 2002; Wang & 

Gao, 2003; Benosman & Vey, 2004; Dwivedy & Eberhard, 2006, Tokhi & Azad, 2008). 

In order to fully exploit the potential advantages offered by these lightweight robot arms, 

one must explicitly consider the effects of structural link flexibility and properly deal with 

(active and/ or passive) control of vibrational behavior. In this context, it is highly desirable 

to have an explicit, complete and accurate dynamic model at disposal. 

In this chapter, we aim to present the details of our investigations concerned with deriving 

accurate equations of motion of a flexible link robot arm by the use of the TBT. 

In the first part of this work, a brief review of different beam theories and especially that of 

Timoshenko is given. Then, based on the TBT, the emphasis is essentially set on a detailed 

description of the different steps, allowing the obtaining of accurate and complete 
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governing equations of the transversely vibrating motion of an actuated lightweight flexible 
link robot arm carrying a payload at its free end-point. To display the most relevant aspects 
of structural properties inherent to the modeled deformable link studied as a beam, 
important damping mechanisms often ignored, internal structural viscoelasticity (Kelvin- 
Voigt damping) and external viscous air damping, are included in addition to the TBT 
effects of cross section shear deformation and rotational inertia. 

In the other part of this chapter, an illustrative application case of the above presentation is 
rigorously highlighted. A new comprehensive dynamic model of a planar single link flexible 
manipulator considered as a shear deformable TB with internal structural viscoelasticity is 
proposed. On the basis of the combined La grangian- Assumed Modes Method with specific 
accurate boundary conditions (BCs), the full development details leading to the 
establishment of a closed form dynamic model, suitable for control purposes, are given. 

2. Timoshenko Beam Theory Based Mathematical Modeling 

2.1 Brief review of beam theories 

A rigorous mathematical model widely used for describing the transverse vibration of 
beams is based on the TBT (or thick beam theory) (Timoshenko, 1974) developed by 
Timoshenko in the 1920s. This partial differential equation (PDE) based model is chosen 
because it is more accurate in predicting the beam's response than the EB beam (EBB) theory 
(EBBT) (Meirovitch, 1986) one (Aldraihem, 1997; Geist & McLaughlin, 2001; Stephen, 2006; 
Salarieh & Ghorashi, 2006). Indeed, it has been shown in the literature that the predictions of 
the TB model are in excellent agreement with the results obtained from the exact elasticity 
equations and experimental results (Trail-Nash & Collar, 1953; Huang, 1961; Stephen, 1982; 
Han et al., 1999; Stephen, 2006). 

Historically, the first important beam model was the one based on the EBT thin or classical 
beam theory as a result of the works of the Bernoulli's (Jacob and Daniel) and Euler. This 
model, established in 1744, includes the strain energy due to the bending and the kinetic 
energy due to the lateral displacement of the beam. In 1877, Lord Rayleigh improved it by 
including the effect of rotary inertia in the equations describing the flexural and longitudinal 
vibrations of beams by showing the importance of this correction especially at high 
frequency frequencies (Rayleigh, 2003). In 1921 and 1922, Timoshenko proposed another 
improvement by adding the effect of shear deformation (Timoshenko, 1921; Timoshenko, 
1922). He showed, through the example of a simply-supported beam, that the correction due 
to shear is four times more important than that due to rotary inertia and that the EB and 
Rayleigh beam equations are special cases of his new result. As a summary, four beam 
models can be pointed out (Table 1), the EBB and TB models being the most widely used. 
As seen above, the TBT accounts for both the effect of rotary inertia and shear deformation, 
which are neglected when applied to EBBT. The transverse vibration of the beam depends 
on its geometrical and material properties as well as the external applied torque. The 
geometrical properties refer mainly to its length I , size and shape of its cross-section such 
as its area A , moment of inertia I with respect to the central axis of bending, and 
Timoshenko's shear coefficient k which is a modifying factor ( k < 1 ) to account for the 
distribution of shearing stress such that effective shear area is equal to kA . The material 
properties refer to its density in mass per unit volume p , Young's modulus or modulus of 
elasticity E and shear modulus or modulus of rigidity G . 
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Timoshenko 
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+ 



Table 1. The four beam models with the corresponding effects 

2.2 The flexible robotic system: Definitions and variables 

The flexible manipulator physical system under consideration is shown in Fig. 1. It consists 
of a pinned-free or a clamped-free with tip payload (see Fig. 2) planar moving flexible arm 
which can bend freely in the horizontal plane. The deflection which is the transverse 
displacement of the link from the X -axis is denoted by w(x, t) . 




Fig. 1. Pinned and clamped configurations of the considered flexible link manipulator arm 

The Fig. 1. is a "top" view of the manipulator in deflection and the axis of rotation of the 
rigid hub ( Z ) is perpendicular to robot evolution plane. The X - Y coordinate frame is 

the inertial frame of reference. The one indicated by X — Y is a frame of reference that 
rotates with the overall structure. 

For the pinned case (Loudini et al., 2007a; Loudini et al., 2007b), the X -axis is intersecting 
the center of mass of the whole system. In the clamped case (Loudini et al., 2006), the X - 
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axis is tangent to the beam at the base (Bellezza et al., 1990). 





Fig. 2. The two different cases: (a) Clamped-Mass, (b) Pinned-Mass 

In Fig. 2., the first case is named Clamped-Mass, meaning that one end is blocked in both 

angular and vertical direction, and the other end is carrying an inertia load. 

The second case is named Pinned-Mass and, as before, it is locked at one end in the vertical 

direction but free to move in the angular like if it were mounted to a rotary actuator that did 

not provide a torque, and carrying an inertia load at the other end. 

Considering, as usual, the flexible link as a beam, its cross-section height is assumed to be 

larger than the base. This constrains deflections to occur only in the horizontal plane. Thus, 

those due to gravity are assumed negligible. 

As depicted in Fig. 1, the robot manipulator is essentially composed of a rigid hub, a flexible 

link and a payload. These three parts are characterized by different physical and mechanical 

parameters (see the nomenclature at the end of the chapter). In particular, the rotating 

inertia of the actuating servomotor and the pinning (clamping) rigid hub are modeled as a 

single hub inertia ] h . The payload is modeled as an end mass M , with a rotational inertia 

/ . rj(t) being the rotating X -axis angular position, the angular position of the hub, 6(t) 
(for the pinned case), and that of a point of the deflected link, a(x,t) , are, respectively given, 
for small deflections, by: 



9(0 = #) + ^^ 



ex 



(pinned case) (1) 



a{X/t) = t]{t) + }^A (2 ) 



2.3 Derivation of the governing equations of motion 

The kinematics of deformation of an element of the deflected link with width dx at position 
x are shown in Fig. 3. Due to the effect of shear, the original rectangular element changes its 
shape to somewhat like a parallelogram with its sides slightly curved. 
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Fig. 3. Kinematics of deformation of a bending element 

This element undergoes a shearing force S(x,t) and a bending moment M(x, t) . On the 
opposite side, which corresponds to a position x + dx , the shearing force ( S + dS ) is 



S(X + dx, t) = S(x, t) + 6S( - X,tS> dx 
dx 



(3) 



Likewise the moment force ( M + dM ) at the position x + dx is 



M(x + dx,t) = M(x,t) + 8M ( x,t } ' dx 
dx 



(4) 



630 Advances in Robot Manipulators 



Note that the total deflection is due to both bending and shear forces, so that the shear angle 
a(x,t) (or loss 
bending y (x,t) 



a(x,t) (or loss of slope) is equal to the slope of centerline (neutral axis) ' less slope of 

dx 



ofr,t)=*&2-y(,,t) (5) 

ox 



The shear force S is given by 

8w(x,t) 



S(x,t) = kAGo(x,t) = kAG 



ox 



■r(x<t) 



(6) 



By considering the "standard linear solid (SLS) model" or Zener model (Zener, 1965), with 
the stress-strain law given by 

V+ C D ^ = E E + K* (7) 

at at 

and assuming linear variations of strain and stress across the beam depth, the total moment 
obtained by integrating first moment of stress across the beam cross section is (Baker et al., 
1967): 

M(x,t) = [l+C D ^JM =I^E + K D ^j Ys (x,t) (8) 

The total internal moment (bending and damping) M is then given by (Banks & Inman, 
1991; Banks et al, 1994) 

M(x,t) = EI^l + K D I^ (9) 

ox oxot 

The equation of motion of the studied single link elastic robot arm can be derived by 
considering both the equilibrium of the moments and the forces. 

Taking moments as positive in the counter-clockwise direction, their summation with 
disregarding the second order term of dx , yields the relation between the spatial change in 
the bending moment and the shear force 

8M(x,t) „, , T d 2 Y(x,t) 

y > =-S(x,t) + pI "" (10) 

ox at 

d 2 y(x,t) 
where the term pi — —: — - stands for the distributed rotational inertia given by the product 
8t 

of the mass moment of inertia of the cross section and the angular acceleration. 
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The relation that fellows balancing forces is 



dS(x, t) _ ^ diu(x,t) = pA _d 2 w(x, t) 



Pl- 



ot 



dt 2 



(11) 



i i a 8w(x,t) „ 8 w(x,t) ■ , i . ■ , 

where the terms A D 5 -, pA — 2 — - represent, respectively, the air resistance force 

and the distributed transverse inertial force. 

Substitution of (6) and (9) into (10) and likewise (6) into (11) yields the two coupled 
equations of the damped TB motion: 



KJ 



3 3 r(^ f ) , rr 5 V(* 



ox dt 



El- 



dx 



{ dx r y dt 2 



(12) 



kAG 



(d 2 w{x,t) dy{x,t)^\ 8 2 w(x,t) 8w(x,t) 
— ; : \~pA — A n 



ex 2 



ox 



dt 1 



dt 



(13) 



If the damping effects terms are suppressed, the classical set of two coupled PDEs 
developed by Timoshenko (Timoshenko, 1921; Timoshenko, 1922) arises: 



EI^m + kAG (^A 



ox 2 



dx 



a !r (x i =0 

r dt 2 



(14) 



kAG\ J-fegfo^) d r( x ' t ) ) P A d2w ( x,t ) -0 

dx 2 dx dt 2 



(15) 



The modeled beam cross-sectional area and density being uniform, equations (14) and (15) 
can be easily decoupled as follows: 



8 5 w(x,t) KJp8 5 w(x,t) d l w(x,t) 
KJ — + EI pi 



8x l 8t 



KG 8x 2 8t 3 



dx* 



1 + 



E K D A D )d i w(x,t) 



KG pKAG 



dx 2 dt 2 



(16) 



p 2 I 3 4 w(x,f) EIA D 8 3 w(x,t) pIA D 8 3 w(x,t) 8 2 w(x,t) 8iu(x,t) 
1 h pA h A n = 



KG 8t* 



kAG 8x 2 8t kAG 8f 



dt 2 



dt 



8 5 Y (x,t) KJp 8 5 Y (x,t) 8YM) r 
KJ + EI pi 



8x 4 8t KG 8x 2 8t i 



5x 4 



E K D A D 



KG pKAG 



d 4 r(*,f) 



8x 2 8t 2 



(17) 



P 2 I 5 4 f(x,Q EIA D 8 3 y(x,t) pIA D 8 3 y(x,t) 8 2 y(x,t) 8y(x,t) 
. + + pA + A n = 



KG 8t l 



kAG 8x 2 8t kAG 8t 3 



dt 2 



dt 
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Similar to the those established in (De Silva, 1976; Sooraksa & Chen, 1998), equation (16) is 
the fifth order TB homogeneous linear PDE with internal and external damping effects 
expressing the deflection w(x,t) . 

We have added to this equation the following initial and pinned (clamped)-mass boundary 
conditions (Loudini et al., 2007a, Loudini et al., 2006): 



Initial conditions: 



w(x,0) = w . 



dw(x,t) 



ct 



(18) 



Pinned end: 



ro(0,f) = 0, 



M{x,t)-J, 



e\v(x,t) 

dxdt 2 



(19) 



Clamped end: 



w(0,t) = , 



dw(x,t) 



ex 



(20) 



Free end with pay load mass: 



dM(x,t) 8 2 w(x,t) 



Bx 



8t 2 



... ,. . 5 3 ro(x,f) 



= 



(21) 



The classical fourth order TB PDE is retrieved if the damping effects terms are suppressed: 

(22) 



&e 4 



5 4 ro(x,f) ( E )8 , w(x,t) pi d l w(x,t) 8 2 w(x,t) 
EI pl\ 1 + — - + ■ + pA = 



KG J dx 2 8t 2 KG dt l 



at 2 



If the effect due to the rotary inertia is neglected, we are led to the shear beam (SB) model 
(Morris, 1996; Han et al., 1999): 



5 4 if(x,t) pIE S 4 w(x,f) d 2 w(x,t) 
EI + pA = 



dx i 



KG 8x 2 8t 2 



ct 2 



(23) 



but, if the one due to shear distortion is the neglected one, the Rayleigh beam equation (Han 
et al., 1999; Rayleigh, 2003) arises: 



8 l w(x,t) e 4 w(x,f) 8 2 w(x,t) 
EI pi + pA = 



a-v 4 



8x 2 8t 2 



ct 2 



(24) 



Moreover, if both the rotary inertia and shear deformation are neglected, then the governing 
equation of motion reduces to that based on the classical EBT (Meirovitch, 1986) given by 
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8 4 w(x,t) 8 2 w(x,t) 
EI — - + pA — - = (25) 

8x 4 8t 2 

If the above included damping effects are associated to the EBB, the corresponding PDE is 

8 5 w(x,t) S'wixJ) d 2 w(x,t) dw(x,t) 

K D I K ~^ + EI K — 1 + P A y —^ + A D ^-^ = (26) 

dx l dt 8x l 8t 2 8t 

The resolution of the PDE with mixed derivative terms (16) is a complex mathematical 
problem. Among the few methods existing in the literature, we cite the following 
approaches with some representative works: the finite element method (Kapur, 1966; Hoa, 
1979; Kolberg 1987), the Galerkin method (Wang and Chou, 1998; Dadfarnia et al., 2005), the 
Rayleigh-Ritz method (Oguamanam and Heppler, 1996), the Laplace transform method 
resulting in an integral form solution (Boley & Chao, 1955; Wang & Guan, 1994; Ortner & 
Wagner, 1996), and the eigenfunction expansion method, also referred to as the series or 
modal expansion method (Anderson, 1953; Dolph, 1954; Huang, 1961; Ekwaro-Osire et al., 
2001; Loudini et al. 2006; Loudini et al. 2007a; Loudini et al. 2007b). 

In the latter one, w(x, t) can take the following expanded separated form which consists of 
an infinite sum of products between the chosen transverse deflection eigenfunctions or 
mode shapes W n (x) , that must satisfy the pinned (clamped)-free (mass) BCs, and the time- 
dependant modal generalized coordinates §,(£) : 

w{x,t) = y\w n {x)5 n {t) (27) 



2.4 Dynamic model deriving procedure 

In order to obtain a set of ordinary differential equations (ODEs) of motion to adequately 
describe the dynamics of the flexible link manipulator, the Hamilton's or Lagrange's 
approach combined with the Assumed Modes Method (AMM) (Fraser & Daniel, 1991; 
Loudini et al. 2006; Loudini et al. 2007a; Loudini et al. 2007b; Tokhi & Azad, 2008) can be 
used. 

According to the Lagrange's method, a dynamic system completely located by n 
generalized coordinates q : must satisfy n differential equations of the form: 



f 8 o 



8L 8D 

— + = F f , i = 0,1,2,— (28) 

dq t 8cj ; 



where L is the so-called Lagrangian given by 

L = T-U (29) 
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T represents the kinetic energy of the modeled system and U its potential energy. Also, in 
(28) D is the Rayleigh's dissipation function which allows dissipative effects to be included, 
and F ; is the generalized external force acting on the corresponding coordinate Cj i . 
Theoretically there are infinite number of ODEs, but for practical considerations, such as 
boundedness of actuating energy and limitation of the actuators and the sensors working 
frequency range, it is more reasonable to truncate this number at a finite one n (Cannon & 
Schmitz, 1984; Kanoh & Lee, 1985; Qi & Chen, 1992). 

The total kinetic energy of the robot flexible link and its potential energy due to the internal 
bending moment and the shear force are, respectively, given by (Macchelli & Melchiorri, 
2004; Loudini et al. 2006; Loudini et al. 2007a; Loudini et al. 2007b): 



If 



dw(x,t) 
dt 



dx - 



\\" 



dy(x,t) 



Pt 



dx 



(30) 



1 r dr(x,t) If r _c 

U = -\EI — — - dx + -\KAG[a(x,t)jdx 

2 J _ dx _ 2 J „ 



(31) 



The dissipated energy due to the damping effects can be written as (Krishnan & Vidyasagar, 
1988; Loudini et al. 2006; Loudini et al. 2007a; Loudini et al. 2007b): 



7f 



D=-\A T 
2 



&w(x,t) 
dt 



41' 



dx+-\KJ 
2 



8 3 w(x,t) 

dx 2 dt 



dx 



(32) 



Substituting these energies expressions into (28) accordingly and using the transverse 
deflection separated form (27), we can derive the desired dynamic equations of motion in 
the mass ( B ), damping ( H ), Coriolis and centrifugal forces ( N ) and stiffness ( K ) matrix 
familiar form: 



d 2 q(t) dqU) 
B—^- + H-^- + N(q(t),q(t)) + Kcj(t) = F(t) 



dt 2 



dt 



(33) 



with q(t) = [0(f) 5,(f) 8 2 (t) ■■■ 5„(f)] T ; F(t) = [r ■■■ 0] T . 

If we disregard some high order and nonlinear terms, under reasonable assumptions, the 
matrix differential equation in (33) could be easily represented in a state-space form as 



[z(t) = A_z(t) + Bu(t) 
\y{t) = C z z{t) 



(34) 
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withu(t) = [r •■■ 0] T ; z(t) = \d(t) 8,(1) •■■ 8„(t) 0(f) 5,(1) ■■■ 5 n (t)T . 

Solving the state-space matrices gives the vector of states z(t) , that is, the angular 
displacement, the modal amplitudes and their velocities. 

3. A Special Case Study: Comprehensive Dynamic Modeling of a Flexible 
Link Manipulator Considered as a Shear Deformable Timoshenko Beam 

In this second part of our work, we present a novel dynamic model of a planar single-link 
flexible manipulator considered as a tip mass loaded pinned-free shear deformable beam. 
Using the classical TBT described in section 2 and including the Kelvin- Voigt structural 
viscoelastic effect (Christensen, 2003), the lightweight robotic manipulator motion 
governing PDE is derived. Then, based on the Lagrange's principle combined with the 
AMM, a dynamic model suitable for control purposes is established. 

3.1 System description and motion governing equation 

The considered physical system is shown in Fig. 4. The basic deriving procedure to obtain 
the motion governing equation has been described in the previous section, and so only an 
outline giving the main steps is presented here. 

The effect of rotary inertia being neglected in this case study, equation (10) expressing the 
equilibrium of the moments becomes: 



8M(x,t) 

ex 



-S(x,t) 



(35) 



The relation that fellows balancing forces is 



dS(x,t) 8 2 w(x,t) 

-pA- 



Px 



ct 2 



(36) 



Substitution of 6 and 9 into 35 and likewise 6 into 36 yields the two coupled equations of the 
damped SB motion: 



v 8 3 r(x,t) s 2 r(x,o 

KJ + EI + kAG 



8x 2 8t 



ex 2 



8w(x,t) 
8x 



■Yi*>t) 



(37) 



kAG 



8 2 w(x,t) 8y(x,t) 



dx 1 



ex 



8 2 w(x,t) 
-pA ^- = 

8t 2 



(38) 



Equations 37 and 38 can be easily decoupled to obtain the fifth order SB homogeneous linear 
PDEs with internal damping effect expressing the deflection w(x,t) and the slope of 
bending y(x,t) 
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d 5 w(x,t) pKJ d 5 w(x,t) d'wixj) pEI d l w(x,t) 8 2 w(x,t) 
K D I K -^--S—^ K -^- + EI K -^-L-L K -^- + pA ^- = (39) 

dx'dt KG dx 2 dt 3 dx 4 KG dx 2 dt 2 dt 2 

8 5 r(x,t) pK D Id 5 r(x,t) d l y(x,t) pEId 4 y(x,t) d 2 r(x,t) 



8x 4 dt KG dx 2 df 



dx 4 KG dx 2 dt 2 



dt 2 




dS 
S(x,t) + — dx 
dx 

M(x,t) + dx 






Fig. 4. Physical configuration and kinematics of deformation of a bending element of the 
studied flexible robot manipulator considered as a shear deformable beam 
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We affect to the equation (39) the same initial and pinned-mass boundary conditions, given 
by equations 18, 19, and 21, with taking into account the result established by (Wang & 
Guan, 1994; Loudini et al., 2007b) about the very small influence of the tip payload inertia on 
the flexible manipulator dynamics: 

Initial conditions: w(x,0) = w , iv,(x,t)\_ =ib (41) 

BCs at the pinned end (root of the link): 

w(x, t)\ = : zero average translational displacement (42) 



,„, ,.i T 8 3 w(x,t) 



dxdt 2 
BCs at the mass loaded free end: 



balance of bending moments (43) 



M(x, t )| _ = : zero average of bending moments (44) 

dM(x,t) 



ex 



8 2 w(x,t) 

■M„ — - 



ct 2 



: balance of shearing forces (45) 



The classical fourth order SB PDEs are retrieved if the damping effect term is suppressed: 

8 i w(x,t) pEI 8 l w(x,t) 8 2 w(x,t) 
EI K -L±-L K -^-L + pA K -^- = (46) 

8x l KG 8x 2 8t 2 8t 2 

8 4 y(x,t) pEI 8 4 r(x,t) 8 2 Y(x,t) 

EI n ' '-±^- + pA n =0 (47) 

5x 4 KG 8x 2 8t 2 8t 2 

Moreover, if shear deformation is neglected, then the governing equation of motion reduces 
to that based on the classical EBT, given by 25. 

If the above included damping effect is associated to the EBB, the corresponding PDE is 

8 5 w(x,t) 3"a;(x,f) 8 2 w(x,t) 

K D I K ~^ + EI K — 1 + P A ^ = (48) 

8x 4 8t 8x 4 8t 2 

To solve the PDEs with mixed derivative terms (39) and (40), we have tried to apply the 
classical AMM which is well known as a computationally efficient scheme that separates the 
mode functions from the shape functions. 
The forms of equations (39) and (40) being identical, w(x,t) and y(x,t) are assumed to 
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share the same time-dependant modal generalized coordinate 5(0 under the following 
separated forms with the respective mode shape functions (eigenfuntions) ®(x) and l'(x) 
that must satisfy the pinned-free (mass) BCs: 

w(x,t) = ®(x)S(t) 

(49) 
y(x,t) = "¥(x)5(t) 

Unfortunately, the application of 49 has not been possible to derive the mode shapes 
expressions. This is due to the unseparatability of some terms of 39 and 40. 
To find a way to solve the problem, we have based our investigations on the result pointed 
out in (Gtirgoze et al., 2007). In this work, it has been established that the characteristic 
equation of a visco-elastic EBB i.e., a Kelvin- Voigt model (given in our chapter by 48), is 
formally the same as the frequency equation of the cantilevered elastic beam (the EB 
modeled by 25). Thus, we can assume that the damping effect affects only the modal 
function 5(0 ■ So, the mode shape is that of the SB model (46, 47). 

Applying the AMM to the PDEs 46 and 47, we obtain 

pEI 
EI$ a (x)5(t)-?—®"(x)5(t) + pA$(x)8(t) = (50) 

KG 

pEI 
EN>"(x)5(t) - —W"(x)5(t) + pAW(x)5(t) = (51) 

KG 

Separating the functions of time, t , and space x : 

W h, (x) 

(52) 



5(0 


(D"'(x) 


T'(i) 


\ 


5(0 


pA p 

— O(x) — — 0>"(x) 

EI KG 


pA p 





The differential equation for the temporal modal generalized coordinate is 

5(0 + 15(0 = (53) 

Its general solution is assumed to be in the following forms: 

5(0 = De' at + De-'"' =Fcos(ot + cp) (54) 

where 

X = a 2 (55) 
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The constants D and its complex conjugate D (or F and the phase (/> ) are determined 
from the initial conditions. The natural frequency a is determined by solving the spatial 
problem given by 

0"'(x) + — <b 2 0" (x) - — <a 2 d>(x) = 
KG EI 

W"(x) + —a 2x i ,H <x)-—a 2 W(x) = 

KG EI 

The solutions of 56 can be written in terms of sinusoidal and hyperbolic functions 

O(x) = Cj sin ax + C 2 cos ax + C 3 sinh bx + C 4 cosh bx 
W(x) = D 1 sin ax + D 2 cos ax + D 3 sinh bx + D 4 cosh bx 

where 



(56) 



(57) 



\2KG \{2KG J EI \ 2KG \{2KG J EI 



The constants C k ,D k ;k = 1,4 of 57 are determined through the BCs 42-45 rewritten on the 
basis of 49, 53 and 55 as follows: 

0(0) = (59) 

W(0) = -— a 2 (D'(0) = -J<D'(0) (60) 

EI 

W(£) = (61) 

M„ , 

®'(t)-W(l) = ^a 2 <D(C) = M<D(0 (62) 

KAG 

By applying 59-62 to 57, we find these relations 

Q=-C 4 (63) 

aD, + bD, = -a/Cj - &/C 3 (64) 

flDj cos fl^ - aD, sin a^ + foD 3 cosh b( + bD 4 sinh bt = (65) 
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(C 1 a-C a M-D 2 )cos(rf-(C 2 a + C 1 M + D 1 )sin(rf + (C3i-C 4 M-D 4 )coshW- 



•••(Cb-C,M-D,)sinhW = 



(66) 



The relations between the unknown constants C k and D k are obtained by substituting 57 
into 38: 



D, 



R-c 



-C 2 ;D 2 



R-a 2 



-c i; d, 



R + & 2 



-Q;D 4 



^R + fo 2 ^ 



(67) 



Q=- 



-D 2 ;C 2 



-D i; C 



D 4 ;C 4 



(R-a 2 ) (R-« 2 ) (R + b 2 ) (R + b 2 ) 



-D. 



(68) 



where R ; 



pa 
KG 



From 63 and 67, we obtain 



D, 



i R+b2 )-, 



b{R~a 2 ) 
From some combinations of 63-69, we find the relations 

a(R + b 2 ) 



b(R-a 2 ) 



-sinhM-sinc 



R + b 2 (a 2 + b 2 )(R + b 2 ) 

cos al cosh b( + '— -sinh bt 

R-a 2 b][R-a 2 ) 



C =C C 



(69) 



(70) 



R ) (R ) R 

— -MC„ \cosaf.- —C 21 +M IsinaP. + MC 21 coshbf. + — C„ sinhW 

R 

Msinhfcl coshW 

b 



C^CaA (71) 
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D, 



R + b 2 (a 2 +b 2 )(R + b 2 ) 
cos at cosh bf. + — ^ — -sinh b( 



R-a 2 



b](R-a 2 ) 



a[R + b 2 ) 
b{R-a 2 )' 



sinh M- sin <rf 



D^D^D, 



(72) 



D, 



aMD 21 - R RD 21 + aM 
sin<rf cos<rf- 



R-a 2 



R-i 



aR 



bM 



b[R-a 2 ) R + b 2 



sinh b( + ■ 



aM 



-coshW 



R-c 



R + b 2 



-coshbi 



D, 



(73) 



= D 4 A 



(74) 



Replacing (63) and (69)-(73) into (57), we obtain 

O(x) = Cj sin ax + C 21 (cos ax - cosh bx) + C 31 sinh bx 
W(x) = D 4 [sin ax + D 21 cos ax + D 31 sinh bx + D 41 cosh bx~\ 

In order to establish the frequency equation, we rewrite the equations 63-66 as fellows 

C 2 +C 4 =0 (75) 

fl/Cj + (R - a 2 )C 2 + b/C 3 + (R + b 2 )C 4 = (76) 

(R-fl 2 )sinfl^C 1 +(R-fl 2 )cosfl^C 2 +(R + b 2 )sinh&CC 3 +(R + b 2 )coshWC 4 =0 (77) 



— cos at- M sin a£ \C 1 + -Mcosai? sina( C 2 + coshW-MsinhW |C 3 ■ 



sinh W-M cosh W \C A = 

b 



(78) 
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Consider the coefficients of the four equations as a matrix C given by 



1 1 

a] R-a 2 b] R + b 2 

CF, CF 2 CF, CF t 

CF 5 CF 6 CF 7 CF 8 



(79) 



In order that solutions other than zero may exist, the determinant of C must me null. This 
leads to the frequency equation 



— R](R + b 2 ) + (a 2 +b 2 ) 
b 



R 



MaJ + —[R + b 2 ) 



cos a( sinh bl + ■ 



■(a 2 +b 2 ) 



MbJ + —(R-a 2 ) 
b 



sinatcoshb£+-- 



R](a 2 +b 2 ) + -R](R-a 2 )-M(a 2 +b 2 ) 2 



(80) 



sin at sinh bl—- 



-(R-a 2 ) + -(R + b 2 ) 



cosafcoshbi = 



3.2 Derivation of the dynamic model 

As explained before, the energetic Lagrange's principle is adopted. 
The total kinetic energy is given by 



T=T,+T h + T, 



(81) 



where T h , 7) and T are the kinetic energies associated to, respectively, the rigid hub, the 
flexible link, and the pay load: 



T h =-J„e 2 (t) 

2 



(82) 



r <4J>- 



x6(t) + 



dw(x,t) 
dt 



-\d(t)w(x,t)t 



> dx 



(83) 



T =-M 



xd(t) + 



8iu(x, t) 
dt 



-[e(t)w(e.,t)] 2 W-] p 



( 



0(f)- 



ct 



dw(x,t) 



dx 



(84) 



The potential energy of the system, U , can be written as 
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2 J i dx 2 J 



■'^sf^-rM 



dx 



(85) 



The dissipated energy D may be written as 






dhv{x,t) 
8x 2 dt 



dx 



Using, for ease of manipulation, the following notations and substitutions 

$ u = $.(£) ; $ a z = ®. 2 (e) ; $; =(D;(^);(D; ( 2 =(D; 2 (^);r i = [^ i 2 (x)dx;T 1 = {^(xWtWdx; 



r 2 = \^{x)dx ;T 2i = [^(x^^x^x;^ = [ 0; 2 (x)dx ;T 3 = [ <S>[(x)<S>' 2 (x)dx; 



T 4 = j"w, e (x)dx;r 4 = fw i '(*) 1 P 2 '(x)<k:;r 5 = [<Df(x)dx ;T 5 = |V;(x)cI> 2 (x)dx ;I" 6 = [xO,(x)dx; 



T 7 = [o^x^^x^x;^ = [(D;(x)W 2 (x)rfx;r 7 = W^xfS^xyix. 

I) 

we obtain 

L = ^ h + } p+ M/- ^pA^d^^ + ^M^ + pAI l )5 1 \t)e 1 {t) + ■■■ 
■■■^{M p <b 2 2 + pAI 1 )5 2 \t)e\t) + {M^ l ^ 2l+ pAI lt )5At)5 2 {t)e\t) + ■■■ 

...(pAr 6i+ M/o 1 ,+/ p o;,)e(05 1 (0+(pAr 6i+ M/<i. 2 , + / p <i.;,)e(f)5 2 (f)+--- 

...i(pAr i?+ M p <D 1 , 2 + / p <D;, 2 )5 1 2 (0 + |(pAr i!+ M p <D 2 , 2 +/ p <t.' 2 , 2 )5 2 2 (0 + - 
-{pAT^ +M p <D 1 ,C 2 , + / p O;,<D;,)fi 1 (f)5 2 (0 + - 

^cr 7i -|KAG(r 3] +r 2i )-|Eir 4i fi 1 2 (f) + - 

... KAGr 7! -|KAG(r 3i+ r 2i )-iEir 42 5 2 2 (t)+- 

•■|xAGr 7 +KAGr 7 -KAG(r 3 + r 2 )-ejt 4 \(t)8 2 (t) 



(87) 
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D = |K D J5 1 2 (f)r 5j + ^K D I5 2 2 (t)T 5i + KJS 1 {tMt)T Sa (88) 

Based on the Lagrange's principle combined with the AMM, and after tedious 
manipulations of extremely lengthy expressions, the established dynamic equations of 
motion are obtained in a matrix form by: 



X(<7) 


B ]2 


B 13 l 


B 21 


B 22 


B 23 


. B =l 


B 32 


V 



0(0 
5,(0 
5 2 (t) 



~0 





" 





H,, 


H 23 





H 32 


W 33 _ 



f"fl(*)" 




X" 




5,(0 


+ 


N 2 


+ 


5 2 (0 




N 3 


I 


N(M) 






~ 


[0(0" 




T 


K 21 


K 2 3 


5,(0 


= 





K 32 


K 33_ 


k(oj 








(89) 



where 



B„ = /„ + 7, + M/ 2 + |pA^ 3 + (m p <V + pAr,, )5, 2 (0 + (m p o 2 , 2 + P Ar 1: )5 2 2 (0 + 

2(M„<D 1 ,<D 2 ,+pAr ii )5 1 (05 2 (0 
B 12 = P Ar 6] + M/d> 1( + J,,®;, ; B 13 = pAi; + M/0 2 , + / p d> 2 , ; B 21 = pAi; + M/Q u + ] P K ; 
B 22 = pAr, + M p <£ 1( 2 + /,,<D;, 2 ; B 23 = pA^ + M p <D 1( <D 2 , + / f O;,<D 2 , ; 

B 31 = P Ar 6: + M/<£ 2 , + J p <£ 2 , ; B 32 = pAI"^ +M,»A + 7,®ii*i, ' B 33 = P Ar 1= + M p <£ 2 , 2 + J p <D 2 , 2 ; 
H 22 = K D IT 5 ; H 23 = K D IT 5 ; H 32 = K D JT 5); ; H 33 = K D IT 5 ; 

Nj = 2(M t lf 2 + P AT t )5 l (t)5 1 (t)d(t) + 2(M p <l> 2 , 2 + P AT t )S 2 (t)5 2 (t)d(t) + 

2(M p <D 1 ,<D 2 ,+pAr 1]: )[5 1 (O5 2 (oe(O+fi 1 (Ofi2(O0(O]' 
n 2 =-(M p <D 1 , 2 + pAr li )5 1 (oe 2 (0-(M p o 1 ,<i' 2 , +pAr 1 j5 2 (oe 2 (0; 
n 3 =-(m p <d 1 ,o 2 , +P Ar lii )5 1 (O0 2 (O-(M p <D 2 , 2 + pAr l2 )5 2 (oe 2 (O; 

K 22 = KAG(r 3 + T 2 ) - 2KAGT 7 + E2T 4] ; K 23 = KAG(r 3 + T 2 ) - KAGT 7 - KAGT 7 + EIT t ; 
K 32 = KAGT 7 + KAGT 7 - -KAG(r 3 + T 2 ) - £7T 4 ; K 33 = 2KAGT 7 - KAG[T, + 1", ) - E7T 4 
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4. Conclusion 

An investigation into the development of flexible link robot manipulators mathematical 

models, with a high modeling accuracy, using Timoshenko beam theory concepts has been 

presented. 

The emphasis has been, essentially, set on obtaining accurate and complete equations of 

motion that display the most relevant aspects of structural properties inherent to the 

modeled lightweight flexible robotic structure. 

In particular, two important damping mechanisms: internal structural viscoelasticity effect 

(Kelvin- Voigt damping) and external viscous air damping have been included in addition to 

the classical effects of shearing and rotational inertia of the elastic link cross-section. 

To derive a closed-form finite-dimensional dynamic model for the planar lightweight robot 

arm, the main steps of an energetic deriving procedure based on the Lagrangian approach 

combined with the assumed modes method has been proposed. 

An illustrative application case of the general presentation has been rigorously highlighted. 

As a contribution, a new comprehensive mathematical model of a planar single link flexible 

manipulator considered as a shear deformable Timoshenko beam with internal structural 

viscoelasticity is proposed. 

On the basis of the combined Lagrangian-Assumed Modes Method with specific accurate 

boundary conditions, the full development details leading to the establishment of a closed 

form dynamic model have been explicitly given. 

In a coming work, a digital simulation will be performed in order to reveal the vibrational 

behavior of the modeled system and the relation between its dynamics and its parameters. It 

is also planned to do some comparative studies with other dynamic models. 

The mathematical model resulting from this work could, certainly, be quite suitable for control 

purposes. Moreover, an extension to the multi-link case, requiring very high modeling 

accuracy to avoid the cumulative errors, should be a very good topic for further 

investigation. 
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Nomenclature 

A link cross-section area 

B inertia matrix 

C D viscoelastic material constant 

D dissipated energy 

E link Young's modulus of elasticity 

F vector of external forces 

G shear modulus 

H damping matrix 

I link moment of inertia 

/,, hub and rotor (actuator) total inertia 

/ , payload inertia 

k shear correction factor 

K stiffness matrix 

K D Kelvin- Voigt damping coefficient 

I link length 

L Lagrangian 

M bending moment 

M , payload mass 

n mode number 

N vector of Coriolis and centrifugal forces 

a vector of generalized coordinates 

S shear force 

t time 

T kinetic energy 

U stored potential energy 

w(x, t) transverse deflection 
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X coordinate along the beam 

$> n (x) n th transverse mode shape 

W n (x) n th rotational mode shape 

a(x,t) angular position of a point of the deflected link 

5 n n th modal amplitude 

£ strain 

v normal stress 

9(t) angular position of the rotating X -axis 

p link uniform linear mass density 

a shear angle 

r actuator torque applied at the base of the link 

a n n th natural frequency of vibration 

a d n th damped natural frequency of vibration 

y{x,t) rotation of cross-section about neutral axis 

rj(t) rotating X -axis angular position 

§ n n th damping ratio. 
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1. Introduction 



Rigid Link Electrically Driven (RLED) robot manipulators are used extensively in 
applications. For RLED manipulators, a hybrid adaptive-learning controller, which do not 
utilize the velocity measurements, is designed and proved that it can be made semi-global 
asymptotically stable (Canbolat et al., 1996). The learning part in that work (Canbolat et al., 
1996) is based on the results given in (Messner et al. 1991). However (Messner et al., 1991) 
neglected the electrical dynamics and the velocity measurements are available. In (Canbolat 
et al., 1996), the system is designed through a high pass filter which produces the surrogates 
of velocity. Later in another work, (Kaneko & Horowitz, 1997) designed a similar controller 
for a robot manipulator using a velocity observer neglecting the electrical dynamics. The 
system in (Canbolat et al., 1996) had not been verified through simulation and experiments. 
Recently, (Uguz & Canbolat, 2006) published the simulation results of the controller 
proposed in (Canbolat et al., 1996) for a sinusoidal desired position. However, a typical 
desired position for a robotic application is not generally sinusoidal. Due to this, more 
general position vectors should be generated. A general task requires a smooth trajectory, 
which starts from an initial position to a final position and repeats this over and over again. 
Such a desired trajectory can be generated in several ways (Fu et al., 1987). In the simulation 
of the system in (Canbolat et al., 1996), desired functions should satisfy the certain 
specifications. For this purpose, the polynomial method given in (Fu et al., 1987) is slightly 
modified in order to accommodate with the requirements of the controller. The 
modifications are necessary due to the continuous third derivative or jerk requirement in 
(Canbolat et al., 1996). Here, we also proposed other methods, which utilize transcendental 
functions. Transcendental function methods give a trajectory that can be continuously 
differentiable up to any order. 

Learning control law is usually used for repetitive tasks in which a certain task should be 
repeated in each cycle. Indeed, the adaptive and learning control schemes are very similar, 
since both strategies are based on the estimation of unknown system dynamics. However, 
the learning control philosophy tries to estimate the unknown time functions instead of 
estimating the unknown constant parameters of the system as in the adaptive control setup. 
The aim of the learning control is to improve the tracking performance of the manipulator at 
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each cycle using the error information obtained during the previous cycles. Thus the 
tracking of a desired trajectory is expected to improve in a period of the specified task 
comparing the results in the previous period (Arimoto, 1986; Messner et al., 1991). The 
control law is adjusted using the tracking error obtained at previous trials. The controller is 
expected to "learn" the unknown dynamics and make the tracking error goes to zero 
(Messner et al., 1991). The research on the design of adaptive control laws which tracks a 
desired trajectory asymptotically for rigid link robot manipulators has been conducted for 
years. The parametric uncertainties for a given system are inevitable for precise control. The 
uncertainties considerably affect the control performance of the system. Adaptive 
controllers, which updates the parameter estimates according to an adaptive update rule, 
tries to achieve the required specifications in the presence of parametric uncertainties (Lewis 
et al., 1993). In the case of robot manipulators, the control should be nonlinear due to the 
nonlinear nature of robot manipulator dynamics. Adaptive control law requires the linear 
parameterization of the system dynamics (Sadegh et al., 1990). However, the learning 
controller is generally used for periodic desired trajectories (Arimoto et al., 1985; Bondi et 
al, 1988; Horowitz et al, 1991; Kaneko & Horowitz, 1992; Kawamura et al., 1988; Kuc et al, 
1992; Qu et al., 1993). (Messner et al., 1991) proposed a new learning algorithm. The 
algorithm is based on the selection of a Hilbert-Schmidt kernel. The uncertainties are 
modeled as an integral equation, which includes the multiplication of the kernel and a 
function that represents the system uncertainties. The learning update rule is based on the 
estimation of the system uncertainties via an update rule for the unknown function in the 
integral equation in terms of the known system variables. The controller makes the system 
follow the desired trajectory asymptotically (Canbolat et al., 1996). 

The simulation of the learning control scheme (Canbolat et al., 1996) could not be achieved 
due to the partial derivatives of the control law with respect to the second time variable 
created by the Hilbert-Schmidt kernel. The two-time variables make the system complicated 
to simulate using traditional simulation packages, such as MATLAB® Simulink and 
SIMNON. In order to simulate the system in Simulink, the second time variable is 
considered to be discrete. Therefore, only the samples of the variables at specified locations 
on the second axis are estimated instead of a continuum of time. However, this process does 
not result a discrete-time system. Instead, the process results a higher order nonlinear 
continuous system through the state variables created due to the time-dynamic nature of the 
control law in both independent time variables, that is, the controller equations include 
partial derivatives with respect to both time variables. Since time is not discretized the 
resulting variables on the second axis has still continuous dynamics with respect to the real 
time. 

In this work, the hybrid adaptive/ learning controller proposed by (Canbolat et al., 1996) is 
simulated. The controller does not need the exact parameter values of the robot 
manipulator. The parameters of the electrical subsystem are updated according to an 
adaptive rule; while the uncertainties in the mechanical subsystem are compensated via 
learning term presented by (Messner et al., 1991) and (Canbolat et al., 1996). The controller 
was designed using a back-stepping technique and follows the desired trajectory 
asymptotically. The system used in the simulation is a rigid-link electrically driven (RLED) 
two-link planar robot manipulator, which is actuated by brushed DC (BDC) motors. The 
controller does not use the link velocities and compensates the electrical subsystem 
parameter uncertainties using an adaptive update law, while compensating the 
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uncertainties in the mechanical subsystem via a learning law. The controller is a partial state 
feedback controller which uses only the link positions and the actuator currents and forces 
the system follow the desired trajectory asymptotically (Canbolat et al., 1996). The controller 
is simulated using the MATLAB® SIMULINK software package. The results of the 
simulation shows that the proposed controller provides the semi-global asymptotic 
trajectory following. 

Robot manipulators are implemented in various types like rectangular, cylindrical, 
spherical, revolute and horizontal joints to achieve the desired movements. From an 
industrial point of view, the Selective Compliance Articulated Robotic Arm (SCARA) type 
manipulators are utilized in the processes such as pick-and-place, painting, brushing, and 
peg-in-hole. In general, a SCARA manipulator has four degrees of freedom. Shoulder, elbow 
and wrist arms are controlled by servo motors while the fourth movement is realized 
pneumatically. 

Various types of robot manipulators are designed according to the required movement 
types but the design of the controller is as important as the design of the mechanical parts. 
Several studies are available in the literature related to the design of controllers for robot 
manipulators employing classical proportional-integral-differential (PID) (Das & Dulger, 
2005), adaptive (Queiroz et al., 1997; Kaneko & Horowitz, 1997), learning (Canbolat et al., 
1996; Horowitz et al., 1991; Messner et al., 1991) artificial intelligence (Golnazarian, 1995; 
Jungbeck & Madrid, 2001) and fuzzy logic algorithms (Lewis et al., 1993). 
Here, we describe the design of the hybrid adaptive repetitive controllers given in (Canbolat 
et al., 1996) and (Horowitz et al., 1991) and generate desired position functions, which 
satisfy the specifications given. However, the computation of derivatives requires the 
manipulation of highly nonlinear transcendental functions. The physical limitations of the 
robot manipulator are not considered in generation of desired trajectories. For a thorough 
position function the physical properties should be considered, such as, maximum velocity, 
acceleration, and jerk. Then a delayed hybrid adaptive repetitive controller (Sahin & 
Canbolat, 2007) is designed based on the method of (Horowitz et al., 1991). Also, the 
controllers are applied to a Serpent-1 model SCARA manipulator used in (Das & Dulger, 
2005) in a simulation environment for a desired path generated according to the 
specifications of the hybrid adaptive-learning controller. Then, the performance of the robot 
with classical PD controller, learning based controller without electrical dynamics and 
adaptive/ learning based hybrid controller are examined by means of simulations. Based on 
the simulation results, the performance of learning based controllers and classical PD 
controller is discussed. 

2. Control Objective 

The objective of this work is to develop a repetitive link position tracking controller for rigid 
link electrically driven (RLED) robot manipulators driven by brushed DC motors. The 
controller compensates for the effects of actuator dynamics. Furthermore, it uses only the 
link position and motor current measurements while compensating for the parametric 
uncertainty throughout the entire mechanical system and eliminating the link velocity 
measurements . 
To facilitate the control law development, the position tracking error is defined as 

e=q tt -q. (1) 
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The parametric uncertainties of the mechanical subsystem are included in c(f) of (11) and the 
unknown electrical subsystem parameters are represented by the following vector 

e e =[el,e T e2 , ... ,<£] r e9t 3 " (2) 



where 



# e ;=[l,,i?,.,^,] r e9? 3 (3) 



in which L„ R„ and Km are the diagonal elements of electrical subsystem matrices L, R, and 
K[„ respectively. The true values of these parameters are not known except it is assumed that 
their upper and lower bounds are known. Whenever these upper and lower bounds are 
referred in the text, we will denote upper and lower bounds of a parameter matrix with the 
subscripts upper and lower, respectively. For example, Li ower <A.mm{L) denotes the lower 
bound for the matrix L, where /Imin(L) is the minimum eigenvalue of the matrix L. 

A dynamic estimate ^, G 5? " is used for 6 e . The parameter estimation error, u is defined 
as follows 

9 e = e e -e e . ( 4) 

In the following section, we will give the details of the control design. The controller will be 
a partial state feedback controller in the sense that it will not utilize link velocity 
measurements to compensate for parametric uncertainties in the system. It is shown that the 
designed controller guarantees the semi-global asymptotic link position tracking. The 
system performance is simulated through a computer code. The code is written for both 
hybrid adaptive-learning controllers for BDC RLED robot manipulators and for the learning 
controller designed in (Messner et al., 1991). The results of the simulations show that the 
controller performs well in terms of error is below some certain value. However, the error 
does not become zero, but it has some average value. This is because of the complexity of 
the control law and the minimum information used to achieve the control goal. 

3. System Model 

3.1 Robot and Actuator Dynamics 

The dynamics of an w-link robot manipulator electrically driven by brushed DC (BDC) 
motors can be expressed as follows: 

M(q)q + V m (q,q)q + G{q) + F d q = K T I (mechanical subsystem) (5) 

LI + RI + K b q = V (electrical subsystem) (6) 

where, 

q, q, q :nxl link position, velocity and acceleration vectors, respectively, 

M(q) :nxn symmetric, positive definite inertia matrix, 

' m W' QJ :nxn matrix of centripetal and Coriolis terms, 

Fd :nxn constant, diagonal, dynamic friction matrix, 

G(q) :wxl gravitational effects vector, 
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r :nx\ torque vector, 

L :nxn diagonal inductance matrix, 

R :nxn diagonal resistance matrix, 

Kb :nxn diagonal back-emf matrix, 

K r :nxn diagonal torque coefficients matrix, and 

v :nxl motor input voltages vector. 

The periodic desired trajectory qd(t) and its time derivatives up to 3 rd order should be 

continuous and bounded (Canbolat et al., 1996). 

The following properties of robot dynamics were utilized in the stability analysis of the 

controller: 

1. For any given vector, x(t), the inertia matrix, M(q), satisfies the following inequality: 

M { \xf < x T M{q)x < M 2 \\xf (7) 

where M\ and M2 are known positive constants that depend on the mass properties of the 
specific robot for which the controller is designed. 

2. The matrix M(q) — 2V m (q,q) is skew symmetric, that is, for any given vector x, we have 

x T (M(q)-2V m {q,q))x = (8) 

3. The Coriolis-centripetal matrix V m is bounded as 

\\V m (q,q)\\ iaa <C c \\q\\ (9) 

where Q is a known positive constant. 

4. The left-hand side of (5) can be written in terms of the desired trajectory as 

w(t) = M{q d )q d +V m {q d q d )q d +G{q d ) + F d q d . (10) 

Since the desired trajectories q d ,q d , q d are periodic with the period T, w(t) of (10) is also 

periodic. w(t), can be expressed as a linear integral equation as shown by (Horowitz et al., 
1991). That is, w(t) can be expressed as follows 



w(t) = JK(t,r)c(r)d r (u) 



where K(t,y) is a known Hilbert- Schmidt kernel and c(y) is an unknown influence function. 
Note that t and /are independent variables. 
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5. (Horowitz et al., 1991) used the kernel of the form 

K(t,y)=f +Y i (f, ll cos(27rmt/T)cos(2n-my/T) + d l:l sm(2xmt/T)sm(2xmy/T)), (12) 
where fi and d/s are scalar constants, which satisfy the conditions, 

Dm~ 2 < | f„, | and Dm~ 2 < \ d,„ | for all m>N with D, N are constants. 

If the kernel of the form given in (12) is utilized, then 

T 

l\\c(r)(d r <M (13) 



where /i is a positive constant. 

Consider the following kernel, which is a Gaussian distribution function, given by 






( (t-r) 2 " 



2(7 



2 
J 



(14) 



where a is a positive design constant. This function satisfies the conditions given in (12) 
(Horowitz et al., 1991). 

If the kernel defined in (13) is used, then the following relations can be shown: 

T 

sup \K 2 {t,y)dy = K<co (15) 

te[0,T] J Q 

7 ( d V 

sup f —K(t,y) dy = k„ < co , (16) 

/ S [0,r] J VCT J 

where srand kj are positive constants. 

3.2 Position Tracking Controller 

To achieve the control objective, the methods proposed by (Burg et al., 1996) and (Horowitz 
et al., 1991) are combined. The design procedure can be summarized as: i) We use a pseudo- 
velocity filter to generate the signals for use as link velocity, ii) since the developed torque is 
a function of motor currents, a desired current signal is designed to force the link position to 
track the desired trajectory (backstepping) and iii) the voltage control input is designed to 
ensure the motor currents tracks the desired current. 

Using the position tracking error defined in (1), the following high pass filter is designed to 
obtain a velocity dependent signal e/ : 

p = -(k + l)p + (k 2 +l)e 

e f = -ke + p ( 17 ) 

p(0) = ke(0) 

where k is a positive gain constant and the auxiliary signal p is used to get two 
implementable equations for the filter. 
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The filtered tracking error is defined as follows 

r/ = e + e + e f . (18) 

Note that, the filtered tracking error, r\, cannot be measured, since the link velocities cannot 
be measured. Based on the dynamics of r/, the auxiliary variable Wi(t) is defined as 

T T 

w l (t) = JK(t,r)c x (r)dr = K; i JK(t,r)c(y)d r (i9) 



Note that the uncertainties in K T is now included in w\(t). The desired current, Id, is designed 
to force the filtered tracking error, //, to zero. 

The desired current, Id, is defined as 

I d =w l (t)-ke f +e (20) 

and the current tracking error, ///, is defined as 

%=Id-I (21) 

where W { (?) is the estimate of w\(t) and is defined as 

T 

w 1 (t) = JK(t,r)c x (t,r)dy (22) 



where C x (t,y) e 9?" is an estimate of c x (y). C x (t,y) is updated according to the following 

rule 

t 

c x (t,y) = \(K(a,y)K L (e(a) + e f (a)))da 



+K(t,y)K L e(t)-K(0,y)K L e(0) (23) 

-\\^-K{a,y)Y L e{a)da. 

where Kl, is an nxn diagonal, positive definite gain matrix. 
The electrical parameter regression matrix is defined as 

Y e 6 e =Lw 2 +(k-l)Le f -kLe + RI + K b (q d +e + e f ) (24) 

where 
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i dt 



(25) 



Based on the current tracking error dynamics the voltage control input is designed as 

v = Y e 6 e + (\\K L f i2 K 2 k nl + k 4 k n2 + k n3 + k n4 + k n5 + lk (26) 



where ||JCl||;2 denotes the induced-2 norm of the matrix Kl and the fc„; (i=l,2,...,5) and Kaie 
positive control gains, and the adaptive electrical parameter update rule is defined as 



O e =r e Yjr ll 



(27) 



with /7,e s Jt 3 " x3 " is a positive definite, diagonal adaptive gain matrix and the electrical 
regression matrix Y c eiR 3 " x3 '< is defined as 



Y e = blockdiag 



w 2i +{k-\)e fl -{k+\)e i 

h 

9 di + e i + e fi 



(28) 



where Wx is the ith element of Wi defined in (25). 

The following theorem can be stated for the tracking performance of the proposed controller 
(Canbolat et al., 1996). 

Theorem 1: If the control gains k„i and k„ satisfy the following conditions 



where 



then 



Ki > ->Aiiax +A frat +K b max 



k > 



h 



— iko)f+i 

V 4 



\ 



\ = min {K Tlower , m { , L lower , A mm (T; 1 ), K rlm , er A mm (K~ 1 )} 
Aj =ma.x<.K Tupper ,m 2 ,L upper ,/i m!iX {i e ),K Tupper A mgx {K l ) 



r 



^3=min \K rlower ,l-Ll 



1 1 1 

h h — 

V %1 %2 %3 J 



^ K 2 K 2 

bupper nipper 



k„4 *«5 



e T e T f rf r,] 9 T e f o c T x (t,y)dy 



e« 8 ", 



(29) 
(30) 

(31) 
(32) 

(33) 
(34) 
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lim||e(0l = 0. (35) 

In the above equations, (.)/ , K . r ar >d (.) upp „ denotes the known lower and upper bounds for the 
eigenvalues of the corresponding unknown parameter matrices, respectively. Similarly, 
<4min(-) and /Imax(-) denotes the minimum and maximum eigenvalues of the matrix in 
parentheses, respectively. 

The proof of this theorem can be found in (Canbolat et al., 1996). For the sake of brevity, the 
proof is omitted here. 

3.3 Delayed Learning Rule 

We define the following delayed update rule for W { (t) in (22) and w 2(f) in (25) similar to 
(Messner et al, 1991): 





w, (0 = J K(t, y)cl (y)dy kT<t<(k + \)T 


(36) 






w 2 (t) = \^K(t,y)\c':(r)dy kT<t<{k + \)T 


(37) 


£ k Ar) 


is defined as 

c k x {y) = c k x -\ r ) + K{kT, r )K L e{kT) 






kT 

+ [ K(<j,y)K L \e(cr) + e f ((j)~\d(j 


(38) 




kT-T 






kT r - 1 

f \—K(<T, r )\K L e(<T)d<T, 





with c (y) = —K(0,y)K,e(0) . Note that the form of (22) and (25) are not changed except 
the estimate of c (t, y) defined in (23) is replaced with c (/) ■ Similarly, all other equations 

are same. One can use the same equations for the controller by changing (22), (23) and (25) 
with (36), (38) and (37), respectively. This definition aims the reduction of computational 
burden for real time applications. One can show that the controller with the delayed 
learning rule of (38) is asymptotically stable using the arguments used by (Messner et al., 
1991) and (Canbolat et al, 1996). 

3.4 Generation of Desired Trajectories 

A proper desired trajectory should be generated for the proposed controller, for 
performance evaluation. A position function for a robot manipulator is a smooth function 
that starts from a certain initial position and ends at a final position. Generally, the 
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acceleration and velocity are required to be continuous and smooth. However, in our case 
the desired trajectory should be continuously differentiable up to the third order. There are 
several methods to generate the desired trajectories. One common method is to separate the 
trajectory into three main parts (initial-lift off (IL), lift off-set down (LS), and set down-final 
(SF)) and impose the continuity conditions at the boundaries. The coefficients of the 
polynomials are to be solved according to the imposed conditions. (Fig. 1). 



Fig. 1. Critical points for a robot end-effector position 

In this section, we propose several methods to determine the desired trajectories. 
Polynomial methods are the modifications of 3-5-3 and 4-3-4 methods given in (Fu et al., 
1987). The naming of the method is based on the degrees of the polynomials, which are 
valid for the IL, LS and SF parts. Since these methods generate functions continuously 
differentiable up to the second order, we should have modified the method to generate 
functions with continuous third time derivatives. The modification is done by increasing the 
degrees of the polynomials. Following the same convention, the modified methods are 
named as 4-6-4 and 5-4-5. Detailed formulae can be found in (Fu et al., 1987). 
Formulation is carried as in (Fu et al., 1987). Let to, h, tj, h be the time boundaries for 
subtrajectories IL, LS and SF (Fig. 2). Let us assign the numbers 1, 2, 3, 4, 5, and 6 for 
forward IL, LS, SF and backward IL, LS, SF subintervals, respectively, and define the 
following dimensionless variable rfor each subinterval as follows 



(39) 



for le\ti-\, U] and f=l, 2, ..., 6. Note that, in a given subinterval [U-i, t,] re[0,l]. With the 
definition given in (39) each subtrajectory can be defined on [0, 1]. Boundary conditions 
become the conditions at r=0 and v=l. 
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Let hi be the polynomial in the i th subinterval. The first and k th derivative of hi can be found 

as 



dh t dh t dz 1 dh t dh i 

dt dz dt t: — t, , dz ' dz 



d ' h. 



+ = A 



d h. 



dt k • dz k 



(40) 




to h h h 

Fig. 2. Critical points of a desired trajectory 



t(, h 



where A, 



1 



t, - 1, 



Since both methods use polynomials of maximum 6 th degree in each subinterval, one can 
write the following general expressions for hi and its derivatives: 

h i (z) = a j6 z +a j5 z +a j4 z +a n z +a i2 z~ +a n z + a j0 
1 



h,(z). 



h t (T): 



k (Z) : 



t ~ t, 



{ti-t,-l} 



-1 6a j6 z 5 + 5a jS z 4 + 4a i4 z^ + 3a a z 2 + 2a l2 z + a a ) 



-l30a ;6 r 4 +20a i5 z i +\2a i4 z 2 +6a B z + 2a !2 ) 



{ti-ti-i) 



-(l20a, 6 r 3 +60a j5 z 2 +24a i4 z + 6a B ). 



(41) 



Let qt, Cji, Cj s ve q/ be the positions at the initial, lift-off, set-down and final positions, 
respectively (Fig. 1). Each polynomial should satisfy the following boundary conditions: 



A.(0) = 


--<lb 






^(0) = 


-K(Q)- 


= fh(0) 


= ( 


A,(l) = 


--1f 






h,{\) = 


-hl(\) = 


-hUl) = 
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h i (l) = h M (0) 

ti t (V> = ti M {0) 

h'(i)=h; +l (o) 

where primes denote the derivative with respect to time. In (42), i should be 1 or 2. (42) 
creates 8 equations for the coefficients. At the initial and final positions, the following 
conditions should be satisfied: 



(43) 



From (43) we have another set of 8 equations. Thus we have 16 equations for 17 unknown 
coefficients. In order to have a unique solution, one can use the position at the lift-off or set- 
down positions. Using the lift-off position, we have the following equation: 

ln(l)=cji (44) 

Since the desired trajectory is periodic, the manipulator should go back to the initial position 
at the end of the period. Due to this, the formulation given in (39-44) should be done twice 
for both reaching the final position and returning to the initial position. The forward and 
backward trajectories may not be symmetric. That is, we are free to select different time 
intervals and different lift-off and set-down positions. We can even use different methods in 
the generation of forward and backward paths. Typically, symmetrical trajectories are easy 
to use in applications. 

There are 8 critical points in a period (Fig. 2). The time instants to, t\, ti, h, tt,, ts, te, tjand the 
corresponding positions qo, q\, q2, qi, q$, qs, q^,, qj are critical points of a desired trajectory. The 
first 4 points qo, q\, qi, qs correspond to initial, lift-off, set-down and final positions of the 
forward path, and the last 4 points qn, qs, qe, q7 correspond to initial, lift-off, set-down and 
final positions of the backward path, respectively. The equalities, 

qo=qi, <?3=<74, (45) 

should be satisfied for a periodic trajectory. For a symmetrical trajectory, the following 
constraints in positions, 

qi=qe, qi=q%, (46) 

and in time 

to=T-t 7 , h=T-t 6 , t 2 =T-t 5 , t 3 =T-h, (47) 

should be satisfied. For each desired trajectory, the following position values are used: 
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qi=0, q i=0. 08, q s =0.92, qf=\, 

in forward path and 

qrl, qr0.92, q s =0.08, qf=0 

in backward path. The corresponding time instants are assigned as follows: 

£0=0,11; h=0,15T; t 2 =0,25T; f 3 =0,3T 

U=0,7T; t b =0,75T; £ 6 =0,85T; t 7 =0,9T 

All position values are in radians, since we used a two-link robot with revolute joints. It is 

possible to select closer lift-off and set-down points. However, in this case the 

subpolynomials may have maxima and minima inside their own subintervals. Typically, a 

robot path should be smooth and monotone increasing or decreasing. 

4-6-4 Method 

In this method, IL and SF polynomials are fourth order. Therefore, 

fl;6=fls=0 

in (41). Furthermore, the initial conditions in (43) requires 

aw=qb, an=ai2=0i3=O 

for forward path and 

04O=0tv 041=042=043=0 

for backward path. The conditions in (42), (43) and (44) give the following matrix equality 
for the unknown coefficients: 
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(48) 



where A/'s are defined in (40). (48) is valid for both forward and backward paths. Solving 
(48) for forward and backward paths, we obtained the following solution (Fig. 3): 
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a u 
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(49) 
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Fig. 3. Desired position and its derivatives with 4-6-4 spline method 



5-4-5 Method 

In this method, we should have 
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a i6 =0 

for IL and SF subintervals in (41) and for the LS subinterval 

fli6=fls=0. 

From the initial conditions in (43), we write 

a\o=qb, 0n=0i2=0i3=O 

for forward path and 

U40-<1b/ 041=042=043=0 

for backward path. 

The conditions in (42), (43) and (44) give the following matrix equality for the unknown 

coefficients: 
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(50) 



where A/'s are defined in (40). (50) is valid for both forward and backward paths. Solving 
(50) for forward and backward paths, we obtained the following solution (Fig. 4): 
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(51) 
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Fig. 4. Desired position and its derivatives with 5-4-5 spline method 

Transcendental Function Methods 

In these methods, an upper and lower bounded, monotone increasing or decreasing 
transcendental function, which is continuously differentiable up to the third order, is used. 
In order to obtain a periodic trajectory, the argument of the transcendental function should 
be a continuously differentiable periodic function up to the third order. Typically, a 
sinusoidal function can be selected as the argument. The advantage of using a 
transcendental function with a periodic argument function is that for each subinterval the 
same function is used. This reduces the formulation time of the trajectory. Also the trajectory 
function never has local maxima and minima in the subintervals, since they are monotone 
increasing or decreasing. However, it is not easy to determine the exact lift-off and set-down 
points. The derivatives of the function may become very complex, as in the hyperbolic 
tangent case. Fortunately, we need only the first order derivatives, because the controller 
uses only the desired trajectory and the desired velocity to compute the input functions, as 
in (24). 



l.Hvberbolic Tangent Method 

Let us use the hyperbolic tangent function for the trajectory. In this method, there is no need 
to consider the forward and backward subintervals of IL, LS, and SF. Instead, we use a 
hyperbolic tangent function with a continuously differentiable (at least up to the third order) 
periodic function as the argument. Indeed the method uses the fact that the hyperbolic 
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tangent function can take values in the interval [-1, 1]. Same function is valid for all times 
and for each subinterval of the trajectory. However, it is not easy to determine the 
boundaries for lift-off and set-down positions. There is no general method to determine 
these points. In this method the desired trajectory is defined as: 

h(t)=b[a+dtanh(ccos{a>t))] (52) 

Where & is a weighting constant in radians, a is the constant that determines the initial 
position, c is the constant that determines the lift-off and set-down positions, d is the 
constant which determines the difference between the initial (ba^bd) and final (ba+bd) 
positions, a> is the angular frequency of the desired trajectory. Note that cosine function in 
the argument is continuously differentiable of any order. The determination method of the 
constant c is trial and error. Typically, c should be selected large enough so that the 
trajectory reaches to its final position and remains there for some time without subjecting 
excessive velocities and accelerations for a pick and place task (Fig. 5). However, a, b, and d 
can be determined according to the initial and final desired positions. One can easily find 
the velocity, acceleration and jerk functions by taking the successive derivatives of (52) as 

v(t) = — h(t) = — bed a>sm(a>t) sech 1 (ccos(tttf)) (53) 

dt 

a(t) = — v(t) = -bcdaf sech 2 (ccos(<y?)) cos(a>?) + 2ctanh(ccos(<y?) sul2 (< y (54) 

j(t) = — a(t) = bcdox" sin(atf)sech 2 (ccos(«0)[l~6ccos(«?)tanh(ccos(ftrf) 

* (55) 

+2c 2 sin 2 (ftrf){ sec n 2 (ccos(of))-2tanh 2 (ccos(&tf))f . 
The jerk expression given in (14) can also be written as follows 

j(t) = — a(t ) = bed a) 3 sin(otf) sech 2 (c cos(of)) [l - 6c cos(<y? ) tanh(c cos(of ) 

dt - (56) 

+2c 2 sin 2 (a>t)\3 sech 2 (ccos(<w0)~2 

2.Error Function Method: 

Here the trajectory is selected as the integral of Gaussian distribution function, which is 
known as the error function, defined as 

2 - dx . (57) 



erf(0 = — ?=fexp -—, 



To get a continuous and periodic function, we use a sinusoidal argument as in hyperbolic 
tangent function. The following function is periodic, continuous and differentiable at least 
up to the third order: 

h(t) = B[A-Derf(Ccos(a>t))~\. (58) 
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The advantage of this function is that the derivatives are in terms of simple exponential and 
sinusoidal functions. Note that the function given in (58), has the initial value of zero, if one 
selects A=D. 
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Fig. 5. Desired position with hyperbolic tangent function (c=10) 



3.5 PD and Learning Controllers 

A frequently used controller in control systems is the classical proportional-derivative (PD) 
controller (Das & Dulger, 2005). The main advantage of the PD controller is that it can easily 
be implemented on simple microcontroller architectures. On the other hand, the 
performance obtained from PD controllers is not satisfying for most of the sensitive 
applications. In this work, PD and learning controllers (Messner et al., 1991) are simulated 
along with the hybrid adaptive-learning controller (Canbolat et al. 1996) in order to compare 
the achieved performance. For this purpose, we repeat the main equations of learning 
controller below. 
First, the main equation of the PD controller is 

V k {t) = K p e{t) + K d d ^- (59) 

at 

where e(t) is the error function, K p is the proportional control coefficient and Kd refers to the 
derivative control constant. 
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The learning algorithm proposed by (Messner et al., 1991) has more complex structure than 
a PD controller. The mechanical part of the robot dynamics can be rearranged as 

M(q)q = 7X0 " K fa 4)4 ~ Gfa) ~ F a (4) (60) 

where T(t) is the control torques applied to the joints and q(t)=[qi(t), qi{t)] T is the position of 
the manipulator, q and q are nxl vectors of joint velocities, and accelerations, respectively. 
The other terms are defined in (1). The following function, which includes the mechanical 
uncertainties, is defined 

w r (t) = M(q d )q d + V m (q d ,q d )q d +G(q d ) (61) 

where q,](t) denotes the desired periodic trajectory vector of the robot links and the dots 
denote the differentiation with respect to time, t. Using these two equations, the following 
error dynamics and the desired compensation control law (DCCL) can be derived 

e(t) = f(t,e) + B(t,e)[w r (t)-w r (t)] (62) 

T(t) = w r (t) + F v e v (t) + F p e(t) + d m (q, q) + q n (e v ) (63) 

where e = | e T e T 1 and w r (t) is the estimate of the influence function, w r (t), that 

compensates for mechanical uncertainties, F P and F p are PD gains, e v (t) is the reference 
velocity error vector, e(f) is the position error vector, d {q, q) is the friction compensation. 
q n {ei) is the nonlinear compensation function. As it can be seen from (63), the repetitive- 
learning algorithm utilizes PD control but also uses W r (t) to compensate for the uncertain 
parameters, thus providing "learning". The position error is defined as 

e p (t) = q d (t)-q(t) (64) 

where qd(t) is the desired trajectory. The reference velocity error function is defined as 

e v (t) = e p (t) + Ze p (t) , (65) 

where A is a positive constant and the nonlinear compensation function is given as: 

I l 2 l I 

( ln( e v) = (7 \ e P \ K| ( 66 ) 

The estimate of the uncertainty function W r (t) is updated with following rules: 
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w,.(t)= \ K(t,t)c(t,r)dT 

o 

8 ^ = K(t,r)K L R T e(t) 
ot 



(67) 
(68) 



where K (t, r) is a function that can be selected by the designer as in hybrid controller, e(t) 
is defined in (63), Ki and R are constant matrices (Messner et al., 1991). 

4. SCARA Robot Model 

The SCARA manipulator considered in this study is an experimental robot that has DC 
servo motors for the movements of elbow and shoulder. The third movement is controlled 
pneumatically. The schematic configuration of the robot is shown in Fig. 6. 

The electrical and mechanical dynamical equations of the manipulator are as follows (Das & 
Dulger, 2005) 
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(71) 



The elements A, B, C, and D in (71) are defined as 
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(72a) 
(72b) 
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(72d) 
(72e) 
(72f) 



where I„i and I 2 are motor currents, V a i and V a 2 are motor voltages, Ti and T2 are motor 
torques, d\ and 02 are link angles and 0„,i and 0,„2 are motor angles of link 1 and link 2, 
respectively. Other physical parameters and their values that appear in (72) for the Serpent- 
1 model SCAR A robot are given in Table 1. 




Fig. 6. Upper view of SCARA robot 



Parameter 


Meaning 


Value 


All' Aj2 


Armature inductances of 
motors 1 and 2 


1.3mH, 1.3mH 


°al' ^a2 


Armature resistances of 
motors 1 and 2 


3.5 Q, 3.5 Q 


A d , ^ e2 


Inverse emf coefficients of 
motors 1 and 2 


0.047 V.s/rad, 
0.047 V.s/rad 


./v tm / JV yo 


Torque coefficients of 
motors 1 and 2 


0.047 Nm/A, 
0.047 Nm/A 
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J U J 2 


Moment of inertias of 
arms 1 and 2 


0.0980kgm2, 
0.0980kgm2 


"ml ' J ml 


Inertias of motors 1 and 2 


3.3.10-6 kgmz, 
3.3.10-6 kgm2 


m x , m 2 


Masses of arms 1 and 2 


1.90 kg, 
0.93 kg 


h, r 2 


Lenghts of arms 1 and 2 


250 mm, 
150 mm 


N U N 2 


Gearbox ratios of motors 1 
and 2 


90, 
220 



Table 1. Serpen t-1 robot parameters and their values 



5. Simulation 



Dynamics of the SCARA robot and three types of controllers, namely PD, learning and 

adaptive/ learning controllers are modelled in MATLAB Simulink environment. A general 

simulation model is given in Fig. 7. 

In the first simulation, the SCARA is controlled by PD controller. In this case, the electrical 

dynamics are neglected and the controller block is replaced with a PD controller (Fig. 7). The 

control coefficients are selected as _K p i=300, Kdi=50, K t ,2 =/ i0, Kdz—\b for link 1 and link 2, 

respectively (Das & Dulger, 2005). 

As the second simulation, SCARA is controlled by learning controller. Here the electrical 

dynamics are again neglected and the controller block is replaced with the learning 

controller designed by (Messner et al., 1991). In the learning controller, the parameters are 

selected as; 

RbbbI Dynamics --- - 
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Fig. 7. Detailed Block diagram of robot and controller 
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F 



2000 
160 



(73) 



F 



200 
4 



(74) 



K L = 



2000 
175 



(75) 



and lj,=10, ve a„=0, d m (x p )=0 (Messner et al., 1991). The computation of C and w r are 

accomplished by numerical integration with embedded function blocks. The learning 
controllers have two different independent dynamic (time) variables. The simulation 
packages do not allow more than one independent simulation variables. To overcome this 
limitation, the second time variable is defined as a discrete variable and at every discrete 
point some state variables are introduced according to the dynamics. The differentiation and 
integration in the second variable are defined through summation and difference equations. 
The result is a heavy computational burden on the system. 

The simulation model of the adaptive/ learning hybrid controller is essentially the same as 
in Fig. 7. The parameters of the adaptive/ learning controller are selected as; fc=15, 0=12 and 



K L = 



100 
100 



(76) 



Again, the computation of C , W l , wi are realized with numerical integrator blocks. 
The desired link angle function is chosen as 

g rf (0 = -0.5 + (-l + tanh(10cos(&>0))' 



(77) 



where a=l rad/s. 

The function given in (77) is a pick-and-place type task that is widely used in industrial 

applications. This trajectory function satisfies the periodicity and continuous 3 rd order 

derivative requirements of hybrid/ learning controller as discussed in section 3.4. 

The desired and achieved link angles when PD controller is used and the link angle errors 

are given in Fig. 8 and Fig. 9, respectively. The maximum angle errors are 0.4 rad for first 

link and 0.65 rad for the second link. 
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Fig. 8. Desired and simulated link angles when PD controller is utilized 
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Fig. 9. Link angle errors when PD controller is used 

Similarly, the link angle errors for learning controller are plotted in Fig. 10. The maximum 
angle errors are 0.09 rad for first link and 0.19 rad for the second link. The angle error 
decreased with respect to PD controller case as it is expected. 

The link angle errors are given in Fig. 11 for the hybrid controller. Note that, the maximum 
link angles are lower compared to learning controller, 0.06 rad for both link 1 and link 2 (the 
error plots for link land 2 are overlapped in Fig. 11). It is worth noting that, the link angle 
errors have greater average values when hybrid controller is used. We think that the 
average value is greater for the hybrid controller, since it uses less information for the 
compensation of the uncertainties comparing with the learning controller given in (63), 
which uses both link positions and velocities. However the hybrid controller uses the 
measurements of link positions and motor currents. Furthermore, the learning controller 
neglects the electrical dynamics and compensates for only mechanical parameter 
uncertainties. On the other hand, the hybrid controller does not neglect electrical dynamics 
and compensates for mechanical and electrical parameter uncertainties. That is, the 
computational burden on the hybrid controller is much more than the learning controller. 
We think that this fact results more error in the average although the maximum error is less. 
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Fig. 10. Link angle errors when learning controller is used 
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Fig. 11. Link angle errors when adaptive/ learning controller is used 



6. Conclusion 

In this paper, the design of the hybrid adaptive/ learning controller is described. Also the 
design of the learning controller proposed by (Messner et al., 1991) is described shortly 
along with a classical PD controller. The simulation model of a SCARA robot manipulator is 
presented and the performance of the controllers are examined through simulation runs. 
The simulation model and its parameters are based on a physical model of a SCARA robot 
given in (Das & Dulger, 2005). The simulation model includes the mechanical subsystem, 
electrical subsystem and the three different types of controllers. The classical PD, learning 
and adaptive/ learning controller schemes are modelled and SCARA robot is simulated with 
three types of controllers. 

The second time variable introduced in learning type controllers results a computational 
burden in dynamics, since the dynamics of controller is dependent both on the real time 
variable and the second time variable created via the Hilbert-Schmidt kernel used in 
learning laws. Moreover, no standard simulation package allows the use of a second 
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independent time variable in the models. To overcome this difficulty, we discretize the 
second variable. In order to keep the dynamics with respect to that variable we should have 
introduced a large number of extra system states at each discrete point of the second 
variable. Although the simulation is sufficiently fast with a high performance (1.7GHz CPU 
and 512MB RAM) personal computer, it is not fast enough with a personal computer of 
lower specifications (667Mhz CPU and 64MB RAM). Considering the much slower 
computers employed for the single task of controlling industrial robots, a real time 
application apparently is not possible at this stage. Therefore, the work to reduce the 
computational burden in the control law is continuing and as soon as this is achieved, an 
experiment to examine the hybrid controller for a real robot will be performed. 
The parameters of a 2-link Serpent-1 model robot are used in simulations and the robot is 
desired to realize a pick and place type movement. According to the simulation results, the 
learning and adaptive/ learning hybrid controllers provided lower angle errors compared to 
classical PD controller. Moreover, the maximum angle errors of links when controlled by 
adaptive/ learning controller decreased from 0.09 rad to 0.06 rad for first link and 0.19 rad to 
0.06 rad for second link compared to learning controller, which means 33.3% and 63.1% 
decrement for first link and second link, respectively. 

Although the hybrid controller is more complex than PD and learning controllers, its 
position and velocity errors have smaller maximum values than the learning controller. 
However its performance is not good in the error averages. We think that the high error 
averages are due to the fact that the hybrid controller uses partial state information (no link 
velocities) and compensates for both mechanical and electrical parameter uncertainties, 
whereas the learning controller uses full state information (both link positions and 
velocities) though it compensates only for mechanical uncertainties, since it neglects 
electrical dynamics. 

Our work is continuing to develop more powerful computational schemes for the hybrid 
adaptive/ learning controller to reduce the computational burden. Recently, we tried to 
introduce a low pass filter in the hybrid controller to filter the high frequency components, 
which effect the tracking performance negatively, in the input voltage. The preliminary 
results show that the error becomes smoother and its average value reduces. 
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